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ABSTRACT 


The  perform  nee  of  a  aodifiad  linear  Kalaan  filter  with 
adaptation  is  coapared  with  that  of  a  coaaon  adaptive 
alpha-beta  filter  for  state  eatiaation  of  a  pilot 
controlled,  ground  directed  boabing  systea.  Of  particular 
concern  is  the  accuracy  and  response  of  the  alternative 
filters  when  the  aircraft  conducts  randoa  aaneuvers  in  the 
vicinity  of  the  target.  The  desirablity  of  including 
deterainistic  forcing  in  the  filter  model  is  discussed  and  a 
technique  utilizing  an  adaptive  Kalman  identifier  to 
establish  the  pilot  response  to  ground  control  heading 


commands  is  presented 
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The  ground  directed  boebing  system  simulated  is 
conceptually  similar  to  the  USBC  AH/TPB-1D  produced  by 
Sierra  Besearch  corporation.  This  systea  tracks  the 
tactical  aircraft  with  a  conical  scan  radar,  filters  the 
noisy  radar  data,  calculates  heading  coaaands  based  on  the 
saoothed  trajectory,  and  transmits  this  guidance  information 
to  the  pilot  via  the  Tacan  navigation  system  located  in  the 
cockpit.  This  heading  information  directs  the  pilot  to  fly 
the  aircraft  so  that  its  ground  track  vector  passes  through 
the  calculated  ordnance  release  point.  Audio  signals 
transmitted  to  the  pilot  designate  the  bomb  release  time. 

In  an  operational  environment  such  a  systea  would 
possibly  be  required  to  track  and  guide  aircraft  conducting 
significant  maneuvers  enroute  to  the  target.  These 

maneuvers  would  most  likely  be  dictated  by  tactical  doctrine 
or  by  the  threat  environment. 

With  this  operational  model  in  mind  an  appropriate 
concern  is  the  capability  of  a  ground  directed  bombing 
system  to  track  and  guide  an  aircraft  exhibiting  random 


maneuvers  until  moments  prior  to  bomb  release.  It  is 


obvious  that  the  smoothing  filter  should  be  able  to  respond 


to  maneuvers,  yet  settle  quickly  to  an  accurate  solution  as 


the  pilot  steadies  the  aircraft.  These  conflicting 
requirements  are  investigated  utilizing  both  alpha-beta  and 
Kalman  filtering  techniques. 

Similar  filtering  techniques  were  utilized  for  a 
simulation  of  the  OS  MC  KN/TPQ-27,  [1].  However  in  that 
ground  directed  bombing  system,  control  signals  were 
directly  coupled  to  the  aircraft  aerodynamic  controls,  thus 
eliminating  the  uncertainty  of  pilot  response  in  the  control 
loop.  In  that  study  significant  improvements  in  filter 
response  and  accuracy  were  realized  by  including 
deterministic  forcing  autopilot  commands  in  the  state 
estimation  via  the  Kalman  filters. 


A.  COORDINATE  SYSTEMS 

A  Cartesian  coordinate  system  was  chosen  for  the 

aircraft  dynamics  model  and  a  radar  centered  inertial 

reference  frame.  In  this  reference  system  the  y-axis  is 

directed  toward  true  North  and  the  x-axis  toward  the  east. 

The  z-axis  is  directed  away  from  the  center  of  the  earth. 

All  radar  measurements  of  aircraft  position,  however,  are 

obtained  in  spherical  polar  coordinates,  i.e.  slant  range 

(R)  ,  azimith  angle  from  true  North  (A)  ,  and  elevation  angle 

(E)  from  the  horizontal.  Figure  2.1  shows  these  coordinate 

# 

systems  and  their  transformation  relationship.  Wind  is 
modeled  with  a  constant  velocity  in  the  x-y  plane  with  no 
vertical  component. 

Curvature  of  the  earth  and  the  fact  that  pilot  heading 
information  is  oriented  to  magnetic  north,  were  not  taken 
into  account  in  the  simulation.  Also  bomb  ballistics  and 
therefore  coriolis  forces  were  not  included  in  the  model. 
The  aircraft  is  simply  directed  to  a  release  point  in  space, 
which  in  a  full  simulation  would  be  derived  from  the 
projected  bomb  trajectory  , ballistic  winds,  coriolis  forces. 


and  a  number  of  other  factors,  all  of  which  are  important  to 


East 

y  »  R  cos  (E)  cos (A) 
z  ■  R  sin (E) 


Pig-  2-1-  Model  Simulation  Coordinate  Systeus 


the  problea  as  a  whole  bat  are  cot  necessarily  geraane  to 
the  objective  of  evaluating  the  response  and  accuracy  of 
alternative  state  estiaating  filters  for  a  goal  oriented 
aaneuvering  boaber.  Thus  the  siaulation  has  been  siaplified 
appropriately. 


B.  AIRCRAFT  DYNAMICS  MODEL 

The  dynaaic  aodel  of  the  aircraft  for  the  purpose  of 
siaulation  was  assuaed  to  be  a  free  inertial  (1/s2)  plant 
since  the  bombing  profile  dictates  a  constant  aircraft 
airspeed.  The  discrete  realization  of  this  plant  is  shown 
in  (2.1). 


X  (k  +  1 )  =  0<k+1/k)X(k)  ♦  A(k+1/k)0(k) 


(2.1) 


where 


0(k+1/k) 


1  T  0  0  0  0 
0  1  0  0  0  0 
0  0  1  T  0  0 
0  0  0  1  0  0 
0  0  0  0  1  T 
0  0  0  0  0  1 


(2.2) 


A(k-HA) 


0  172  0 

0  T  0 

0  0  T/>2 

0  0  T 


(2.3) 


In  the  controlled  mode  the  aircraft  model  simulates  the 
pilot  responding  to  heading  inpats  transeitted  from  the 
radar  site  to  the  TACAN  navigation  system  in  the  cockpit. 
The  model  is  driven  by  a  pilot/aircraft  control  function 
similar  to  That  developed  in  [ 1 ]  and  shown  in  Figure  2.2. 

The  input  is  bearing  to  the  target  and  the  output  is  a  bank 
angle  which  generates  a  heading  rate  that  can  be  transformed 
into  x-y  acceleration  components  for  entry  into  the  dynamic 
model.  It  is  assumed  that  in  the  conTrolled  mode  heading 
changes  are  made  wicn  coordinated  turns  performed  by  the 
pilot  in  response  to  heading  commands  displayed  by  the 
TACAN.  The  pilot /aircraft  controller  induced  x-y 
accelerations  are  depicted  in  Figure  2.3  and  summarized  by 
(2.4)  and  (2.5)  below. 


x(k)  *  7  (k)  cos(^(k) )  ^(k) 


(2.4) 


y(k)  *-7  (k)  sin(^(k)  )  i£(k) 


(2.5) 
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Fig.  2.2.  Pilot/ lircraft  Controller  Configuration 


^(k)  and  V  (k)  are  aircraft  heading  and  velocity  respectively 
at  time  k,  and  ^(k)  is  the  heading  rate  which  is  derived  in 
(2.6)  through  (2.9)  from  the  free-body  diagram  shown  in 
Figure  2.4.  The  aircraft  weight  is  shown  in  (2.6)  below. 


W  =  jg  =  L  cos  9 


(2.6) 
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Fig.  2.3.  Pilot  Induced  X-T  Accelerations 


Equation  (2.7)  depicts  the  centripetal  force  generated  in  a 
turn,  where  R  is  the  turn  radius,  L  is  lift,  7  is  velocity, 
and  9  is  the  bank  angle. 

P  *  aV/'R  =  L  sinP  (2.7) 

%• 

Eut  7/R  » i If  ,the  turn  rate,  so 


^  -*•  V  V  -•  --•  V>  .•  V  -•.  -•  V  .Vj  /.  »  .  »  .  t*  V*.  - 


Fig.  2.4.  Coordinated  Turn  Free* Body  Diagrae 

Dividing  (2.8)  by  (2.6)  and  rearranging  teras  yields  (2.9), 
which  defines  &  ,  the  aircraft  turn  rate,  as  a  function  of 
aircraft  bank  angle  9,  and  velocity  V. 

^  *  g/v  tan 0  (2.9) 

From  [1]  the  aircraft  roll  response  is  assumed  to  be  of 
the  fora  shown  in  (2.10)  where  T  is  the  roll  response  time 


constant. 


1/(ST  ♦  1) 


(2.10) 
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So  effort  has  been  Bade  to  specifically  aodel  pilot  delays 
or  response  to  visual  inputs  fros  the  TACIS. 

In  the  maneuvering  eode  the  maneuver  aodel  described  in 
(2)  vas  used  and  is  shown  in  Figures  2.5  and  2.6  . 


Fig.  2.5.  Acceleration  Probability  Density  Hodel 

This  was  simulated  for  uncontrolled  random  flight  since  the 
aircraft  is  assumed  to  typically  move  at  a  constant  velocity 
with  turns,  evasive  maneuvers,  and  air  turbulence 
interpreted  as  perturbations  upon  the  constant  velocity 
trajectory.  These  maneuver  perturbations  or  accelerations 


Pig.  2.6.  Bod el  Acceleration  Correlation  Function 

can  be  specified  by  a  magnitude,'  with  probability  P(a)  from 
(2.11),  and  duration  of  a  (a)  from  (2.12),  the  correlation 
function  of  aircraft  acceleration. 

P(a)  *  (1-(2Pj  ♦  P0))/2A  (2.11) 

R ( a)  *  a  exp  (-t|  a{ )  (2.12) 

The  acceleration  k  in  (2.11)  is  the  maximum  that  can 
reasonably  be  expected  from  the  pilot/aircraft  in  the 

environment  described.  Pj  is  that  probability  assigned  to 

the  maximum  acceleration  ±A,  PQ  is  that  probability  assigned 


9 


to  no  maneuver,  and  the  assaeed  probabilitity  distribution 
between  these  values  is  unifora  with  amplitude  P  (a)  . 
Equation  (2.12)  is  the  correlation  function  which  yields  an 
acceleration  tiae  duration*  R  (a)  *  which  is  based  on  the 
aagnitude  of  the  acceleration  |a|.  M  and  t  are  siaply 
correlation  factors  which  determine  how  the  R{a)  varies  over 
the  range  of  possible  acceleration  amplitudes.  Proa  this 
aodel  one  can  see  how  the  duration  of  a  high  G  maneuver  for 
threat  avoidance  would  be  considerably  less  than  for  a  low  G 
clearing  turn. 

C.  GDBS  MODEL  IMPLEMENTATION 

The  GDBS  model  was  simulated  on  an  IBM  370  in  single 
precision  Fortran.  Figure  2.7  shows  the  basic  flow  diagram 
for  the  computer  program,  which  implements  this  simulation 
model.  The  module  labeled  'State  Estimation  Filter' 
represents  those  filter  algorithms  discussed  in  the  next 
chapter. 


ill.  illQUll  SlkU  SS imUQS. 


A.  BACKGROUND 

A  great  deal  has  been  written  on  the  theory  and 
application  of  estiaation  filters.  In  particular,  £2] 

provides  a  good  overview  of  several  such  filters,  including 
the  alpha-beta  and  Kalman  filters,  and  compares  their 
relative  performance  not  only  in  terms  of  accuracy  and  in 
response,  but  also  in  terns  of  computer  implementation  costs 
in  computation  time  and  storage  overhead. 

The  general  conclusion  is  that  the  Kalman  filter 
out -performs  an  alpha -bet a  filter  of  comparable  order  by 
about  2  to  1.  However,  the  cost  for  such  performance  is 
increased  computer  computation  time  and  memory,  of  the  same 
relative  magnitude. 

B.  GENERAL  DESCRIPTION  OF  THE  ALPHA-BETA  FILTER 

The  basic  theory  of  the  alpha-beta  filter  is  derived 
from  minimizing  the  mean  square  error  of  the  filtered 
states.  A  classic  analysis  of  the  alpha-beta  filter  is 
provided  by  [3].  The  filter  recursive  equations  are 

summarized  below. 

x(k/k-1)*x  (k-1/k-1)  ♦  T  x  (k-  1/k-  1)  (3.1) 


22 
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x  (k/k)  »  x  (kA-1)  ♦  a  (z  (k)  -  x(kA-1) 


(3.2) 


i(k/k)  *i(kA-1)  ♦  0/T  (z(k)  -  x(k/k-1))  (3.3) 


x(k/k-1)  is  the  predicted  position,  X  (k/k)  is  the  updated 
position,  x  (k/k)  is  the  updated  velocity,  and  *(k)  is  the 
noise  contaainated  aeasureaent  of  position  at  the  k-th 
interval.  T  is  the  saaple  rate  of  the  aeasureaent  process, 
a  and  fi  are  usually  fixed  real  constants,  As  pointed  out  in 
[  4  1  these  alpha-beta  equations  are  analogous  to  a  steady 
state  Kalaan  filter.  Por  typical  parameter  values  the 
alpha-beta  filter  is  simply  low  pass  with  a  heavily  damped 
time  response.  Thus  the  filter  eliminates  not  only  most 
high  frequency  measurement  and  process  noise,  but  also  most 
maneuver  energy  from  the  state  estimate. 


C.  GENERAL  DESCRIPTION  OF  THE  KALMAN  FILTER 

The  Kalman  filter  generates  a  minimum  variance  estimate 
cf  the  plant  (aircraft)  state  vector  when  the  measurement  and 
plant  process  noise  statistics  are  known  and  conform  to  the 
criteria  shown  below. 


E(V(k)7(  j)1)  =  R  (k)  5  (k,  j) 


(3.4) 


B(A<«00  8(j)T)AT)  »  Q(M8(k,j) 


(3.5) 


*<▼(*)*  (j)  )  *  0  for  all  k,j 


(3.6) 


a 


where 


S(kt  j) 


0  k*j 
1  k*j 


(3.7) 


A  linear  time- invariant  system  is  assumed,  as  in  the 
discrete  aodel  representation  shown  in  Figure  3.1.  X(k) 
represents  the  (n  x  1)  state  vector,  Z  (k)  the  (a  x  1)  output 
vector',  0(k)  the  state  transition  aatrix,  H  (k)  the  (a  x  n) 
observation  aatrix,  (f(k)  state  excitation  or  process  noise, 
and  V  (k)  the  aeasureaent  noise. 

The  Kalaan  filter  recursion  algoritha  is  suaaarized 
below. 


X  (k+  1)  *  tf(k)X(k)  ♦  A(k)  W  (k) 


(3.8) 


Z(k)  3  H  (k)  X  (k)  ♦  V (k) 


(3.9) 


X  (k/k-  1) 


»  0(k,k- 1)  X  (k-  1/k- 1)  ♦  A(k)U(k-1)  (3. 1C) 


$ (k/k)  »  X(K/K-1)  +  G  (k)  |z  (k)  -  H(k)X(kA-1)|  (3.13) 


P(k/k)  =  (I  -  G  (k)  H (k) )  P  (k/k- 1) 


(3.14) 


X(k/k-1)  denotes  the  estimate  of  the  state  vector  X(k)  based 
on  (k-1)  measurements,  Z(1),Z(2), . Z(k-1). 


D.  FILTER  ORDER  CONSIDERATIONS 

The  filter  order  is  chosen  to  match  as  closely  as 
possible  the  expected  plant  dynaaics  of  the  systea  being 
aodeled.  A  first  order  filter  would  be  expected  to  estimate 
a  constant  velocity  trajectory  effectively  and  a  second 
crder  filter  would  accordingly  observe  a  trajectory 
exhibiting  constant  acceleration.  The  order  is  used  here  in 
the  mathematical  sense  and  refers  to  the  order  of  the 
differential  eguation  that  defines  the  filter. 

Since  the  aircraft  is  known  to  be  constrained  to  a 
constant  velocity  profile  as  it  approaches  the  release 
point,  it  would  seem  reasonable  to  select  a  first  order 
filter  for  modeling.  The  aircraft  dynamics  are  anticipated 
to  depart  from  the  first  order  model  enroute  to  the  target 
thus  creating  transient  errors  which  must  be  dealt  with  by 
filter  adaptation.  The  alternative  to  this  srrategy  is  to 
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TIME  (SEC) 

Pig.  3.3.  Second  Order  Kalian  Filter  Sain  Schedule 


E.  FILTER  ADAPTATION 


No  matter  what  the  order  or  the  complexity  of  the 
filter  type  selected,  it  cannot  be  expected  to  fully  model 
the  aircraft  dynamics  and  process  noise  covariance.  The 
model  is  based  on  a  linear  time-invariant  system  and  the 


process  noise  is  ass  need  to  be  stationary,  white,  and 
Ganssian.  Daring  portions  of  the  flight,  paricalarly  as  the 
aircraft  approaches  the  release  point,  the  systee  dynaeics 
are  expected  to  approx ieate  very  closely  the  assneed  aodal. 


However,  during  other  portions  of  the  flight  the  aircraft 
dynaeics  are  anticipated  to  depart  significantly  froe  the 
filter  eodel.  Certainly  the  pilot  should  not  be  constrained 
to  behave  in  a  Banner  consistent  with  the  eodel,  if  the 
environment  dictates  otherwise. 

Since  the  pilot/aircraft  dynamics  are  not  fully 
modeled,  the  subopt imal  filter  that  results  might  be 
expected  to  diverge,  e.g.  the  error  covariance  generated  by 
the  filter  and  the  actual  error  covariance  become 
inconsistent.  The  desire  is  for  the  filter  to  transition 
smoothly  between  accurate  estimations,  when  the  aircraft 
dynamics  conform  to  those  assumed  for  the  model,  and  less 
accurate  estimations,  when  the  aircraft  dynamics  do  not 
agree  with  the  model.  An  adaptive  filter  realizes  this 
smooth  transition  by  adjusting  filter  parameters  to  vary  the 
filter  bandwidth  to  allow  a  more  consistant  match  between 


the  calculated  and  actual  filter  error  covariances. 


In  the  case  of  Kala&n  filter  adaptation,  the 
calculated  error  covariance  becoaes  a  function  of  the 
aeasured  data  indirectly  by  making  the  filter  paraaeters 
dependent  on  the  observed  aircraft  notion.  The  adaptive 
techniques  for  the  alpha-beta  filter  are  siailar  in  concept. 
In  either  case  the  adaptive  process  is  conceptually  straight 
forward;  first  divergence  is  detected,  then  the  filter 
paraaeters  are  modified. 

In  the  case  of  a  ground  directed  bombing  system  of 
the  type  considered  here,  the  aircraft's  behavior  could 
depart  from  the  filter  model  in  a  random  fashion  when  the 
pilot  maneuvers  in  response  to  a  random  event  it  the 
environment,  or  deterministically  when  he  responds  to  target 
tearing  inputs  from  the  ground  radar.  These  two  situations 
may  be  treated  separately  or  together  for  the  purpose  of 
filter  adaptation.  To  treat  them  separately,  as  random  and 
deterministic  processes,  requires  knowledge  of  the 
pilot/aircraft  response  to  target  bearing  inputs.  In  the 
case  of  the  ground  directed  bombing  system  described  in  [1] 
this  transfer  function  was  known  quite  accurately  since  the 
input  signals  from  the  ground  radar  were  directly  coupled  to 
the  aircraft  aerodynamic  controls,  with  the  uncertainties  of 


pilot  response  isolated  froa  the  control  loop.  With  that 
infornation  the  deterministic  forcing  could  simply  be 
integrated  into  the  filter  aodel.  Unfortunately  such  is  not 
the  case  for  this  siaulation.  A  unique  approach  to  this 
problea  will  be  discussed  later.  The  alternative  approach 
is  to  consider  both  processes  to  be  random  and  proceed  froa 
that  assumption. 

2.  maiasigaa . sisHs&Lssl -laA-JaasaisE . m a 

As  descibed  in  [5]  the  innovations  or  residual 
sequence  of  a  filter  can  be  observed  in  order  to  detect  a 
bias  that  would  indicate  divergence  of  the  state  estimate 
froa  the  true  state.  This  is  given  by 

I/(k/k-1)  *  Z(k)  -  H  (k)  £(k/k-1)  (3.15) 

By  substituting  for  Z  ( k)  from  (3.9),  the  measurement  aodel, 
we  see  that 

j/(k/k-1)  =  v (k)  -  H(k)«(k/k-1)  (3.16) 

where 

€(k/k-1)  a  X(k/k-1)  -  X  (k)  (3.17) 

Taking  expected  values  ,  we  find  that 


E  ( j/(k/k-  1) ) 


0 


(3.19) 


•  j 


E(V(k/k-1)  pT(kA-1))  »  R(k)  ♦  R  (k)P  (k/k-1)  HT(k)  (3.19) 

Thus  by  referring  to  the  eodel  statistics  for  R(k)  and 
P(k/k-1)#  it  becoaes  clear  that  when  the  systea  conforas  to 
the  systea  aodel ,  i.e.  the  filter  is  operating  optimally, 
the  innovation  sequence  should  be  zero-mean  Gaussian  with 
variance 

<r2  *  <y2(k)  ♦  P(k/k-i)  (3.20) 

s  One  appraoch  to  adaptation  considers  the  correlation 

of  the  innovation  sequence,  vher9  the  autocovariance 

Qv( i)  *  E(|/(k/k-1)  v(k-i/k-1-i)T )  (3.21) 

should  vanish  for  i*0.  Based  on  these  statistics,  maneuver 
detection  can  be  realized  by  observing  the  signs  of  the 
innovation  sequence.  The  probability  that  a  given  sequence 
is  either  positive  or  negative  is 

P(  0  >  V  >  0  )  *  •  5N  (3.22) 

1-N 

Another  approach  utilizes  (3.23) 

|  (k)|  >  C  a  (k)  (3.23) 

to  declare  a  maneuver  when  exceeds  a  specified  value, 


usually  two  or  three  standard  deviations  of  a.  . 


Subsequent  to  maneuver  detection,  the  filter 
parameters  aust  be  aodified  to  correct  filter  divergence. 
Reference  (51  suaaarizes  numerous  techniques,  soae  being 
quite  coaplez  and  coaputation  intensive.  The  strategy 
chosen  for  this  simulation  was  simply  to  reset  the  error 
covariance  in  response  to  a  detected  maneuver.  In  the 
situation  of  a  ground  directed  bombing  systea,  the  resulting 
cost  of  a  false  detection  becomes  high  only  as  the  aircraft 
approaches  the  release  point.  This  cost  can  be  reduced  by 
disabling  filter  adaption  within  a  specified  time  to  go. 

Still  another  approach  attempts  to  adapt  the  filter 
bandwidth  by  adjusting  Q(k)  in  (3.11).  This  approach, 
investigated  in  (11  and  [6]  calculates  Q  (k)  by 

Q(k)  x  a  Del  (  k)  Del(k)  ♦  b  Del  (k-1 )  Del  (k- 1)  (3.24) 

where  a  and  b  are  determined  by  data  analysis,  and 

Del  (k )  =  X  ( k/k)  -  $(k/k-1)  (3.25) 

A  variation  of  this  technique  that  looks  at  only  the  change 
in  the  highest  order  state  is  investigated  in  the  Kalman 
filter  simulation. 

Host  of  the  discussion  thus  far  concerning  filter 


adaptation  has  been  directed  toward  Kalman  filters. 


Host 


I 


approaches  to  adapting  alpha-beta  filter  simply  open  the 
!  bandwith  by  switching  to  a  different  set  of  parameters  when 

a  a  an  euver  has  been  detected.  Reference  [•]  discusses  an 
adaptation  scheme  that  is  more  nearly  optimal  in  the  sense 

I  ' 

of  covariance  matching.  However,  for  the  sake  of  comparison 
the  simple  parameter  switching  technique  is  implemented  in 
I  the  alpha-beta  filter  simulation  subroutine,  since  that  is 

the  approach  used  in  the  AN/TPB- ID. 

F.  ESTIMATION  OF  PILOT  RESPONSE  TO  TARGET  BEARING  INPUTS 

As  discussed  in  the  previous  section,  aircraft  dynamics 
depart  from  the  filter  model  randomly  when  the  pilot 
responds  to  events  in  the  environment  and  deterministically 
when  he  responds  to  target  bearing  inputs  from  the  GDBS.  If 
his  response  to  these  inputs  were  known  with  some  degree  of 
certainty,  then  deteministic  forcing  might  be  included  in 
the  filter  model  in  a  manner  similar  to  that  found  in 
and  [1].  The  importance  of  identifying  parameters  which 
define  a  system  so  that  modern  control  strategies  can  be 
implemented  is  discussed  in  [7].  In  this  case  the 
paramexers  would  be  those  that  describe  the  pilot/aircraft 


response  to  heading  inputs 


By  using  an  Autoregressive  Moving  Average  (ABBA) 
representation  for  the  pilot/aircraft  system,  and  Kalsan 
filtering  to  process  the  heading-in (actually  heading  error 
froi  the  vein  point  of  the  pilot),  bankangle  out  data,  the 
coefficients  associated  with  the  ARBA  equation  could  be 
identified.  Proa  [7]  we  know  that  the  pilot/aircraft  system 
can  be  represented  by  the  ARBA  equation 


m  n 

*<k )  »  la  6L(k-5)  -  £b.  *(k-j) 

j=o  J  *  j=i  -> 


(3.26) 


where  the  present  bankangle  output,  9  (k)  ,  is  a  linear 

combination  of  past  outputs,  0{k-3),  and  of  past  and 

present  heading  error  inputs,  9  (k) .  Estimating  the 

'  < 

coefficients  of  this  ARBA  equation  can  be  formulated  as  an 
adaptive  Kalman  identifier,  where  the  heading  error  and  the 
bank  angle  are  simple  functions  of  the  velocity  and 
acceleration  state  estimates  generated  by  the  Kalman  state 
estimation  filter  previously  discussed. 

If  the  a  ^  and  b^  coefficients  of  the  AHMA  equation  are 
treated  as  states  of  the  pilot /aircraft  system,  then  the 


state  vector  becomes 


8@  assume  that  these  coefficients  experience 
perturbations  so  that 


(k+1)  *  ai(k)  ♦  (k) 

bj  (k+1)  *  b  j  (k)  ♦  v  j  (k) 

Equation  (3.26)  then  becomes 

m  n 

0(k)  =  £a  A( k-j)  -  £b  d(k-3)  ♦  v(k) 

j=o  3  4  j=l  3 

where  w^k),  w^(k),  v  (k)  are  noise  processes  that 
same  statistics  described  for  the  Kalman  filter 
Combining  (3.28)  and  (3.29)  we  have 


—  — 

a  (k+1) 

a  (k) 

b  (k+ 1) 

b  (k) 

—  mm 

_  _ 

w(k) 


The  measuraent  vector  is  defined, 

H(k)  =  |%((k)  $'{ k-1)  •  •  .  fl>(k-m)  - 

-  0(k-1)  ...  -  0(k-n)l 

From  C71  the  solution  is  then  formulated  as 


r 

— 

- 

|  (k+1/k) 

*  [i  -  G  (k)H(k)] 

A 

a 

( k/k- 1) 

l  <fc*l/k) 

£ 

(k/k-  1) 

—  — 

♦  G(k)  0(k) 


where 


G  (k)  =  ?(k/k-1)  ffr(k)  [h  (k)  P  (k/k- 1)  HT(k)  ♦  r] 


-1 


ra  ndom 

(3.28) 

(3.29) 

have  the 
earlier. 

(3.30) 


(3.31) 

(3. 32) 

(3.33) 
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P  (k/k)  *  P  (k/k-1)  -  G(k)H(k)  P  (k/k-1)  ♦  Q 


(3.3a) 


lv»]  1 


<*)  »j(Mi 


(3.35) 


B  *  E  [  v  (k)  v(k)] 


(3.36) 


P  (k/k-  1) 


a  (k)  _  | (k/k)  a  (k)1  _  \i 

b(k)  "  £  (k/k)  b(k)J  [f 


(k/k) 

(k/k) 


(3.37) 


Initialization  of  the  sta  tes(coef  ficients)  and  the  error 
covariance  would  be  similar  to  that  discussed  in  the  next 


section. 


5.  FILTEH  IMPLEMENTATIONS 

Three  separata  filter  subroutines  were  developed  to 
simulate  the  filtering  cf  raw  radar  data  generated  by  the 
ground  radar  of  the  bombing  system  previously  described. 
All  three  filter  configurations  are  oriented  in  the  three 
dimensional  Cartesian  coordinate  system  described  in  Chapter 
2  since  the  aircraft  dynamics  are  assumed  to  be  more  nearly 
linear  and  well  behaved  than  in  the  polar  coordinate  system 
in  which  the  measurements  are  generated.  This  disparity 


between  the  measurement  reference  frame  and  the  model 
dynamics  reference  frame  results  in  a  basic  nonlinearity 
when  transformations  are  required  from  one  frame  to  the 
other.  Probably  a  better  coordinate  frame  for  modeling 
aircraft  motion  would  be  one  that  translates  with  the 
aircraft  and  is  oriented  along  the  velocity  vector.  Such  a 
coordinate  system  was  found  to  be  very  awkward  and  difficult 
to  implement,  especially  considering  the  problem  of  the 
transformation  nonlinearity  just  mentioned. 

The  first  of  these  filters,  designated  ALFBTA ,  is  a 
simple  sixth  order  alpha-beta  filter  with  the  parameter 
switching  adaptation  tecnique  described  in  the  previous 
section.  Adaption  is  initiated  when  a  heading  rate  cf  1 
degree  per  second  is  observed  for  period  of  5  seconds  or 
more.  The  second  and  third  subroutines  implement 

sixth  (KALB Ml)  and  n  inth  (KALMN 2)  order  Kalman  filters 
respectively.  Two  separate  adaptive  techniques,  which  were 
described  in  the  previous  section,  are  included  with  each  cf 
these  filters.  The  first  of  these  adaptive  algorithms, 
designated  ADPTV1,  adjusts  the  Q(k)  matrix  from  changes 
computed  in  the  highest  order  estimate.  The  second 
algorithm,  designated  ADPTV2,  simply  resets  the  covariance 


of  error  a&trix  P  (k)  when  a  bias  is  detected  in  the 
innovations  sequence  for  aore  than  one  second,  As  aentioned 


before,  the  difference  between  the  sixth  and  ninth  order 
filters  is  that  the  foraer  do  not  estimate  the  acceleration 
states  of  the  aircraft.  The  cost  of  this  additional 
information  provided  by  the  ninth  crder  filter  is  aore 
computation  tine  and  computer  memory. 

The  aircraft  model  is  formed  by  defining  a 

three-dimensional  Cartesian  state  vector 

X3*  [x  y  z]T  (3.38) 

where  x,  y,  and  z  are  each  one-dimensional  two  element  state 
vectors  (position  and  velocity)  for  ALFBTA  and  KALHN1,  and 
three  element  state  vectors  (position,  velocity,  and 
acceleration)  for  KALMN2.  The  state  prediction  equations 
are  given  by  (3.1)  for  the  alpha-beta  filter  and  (3.10)  for 
both  Kalman  filters.  For  the  Kalman  filter  configuration 


(3.39) 
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4  <k) 


(3.40) 


4  0  0 
0  4  0 
0  0  4 


where  ♦  (k)  and  4(k)  are  defined  by  (3.41)  and  (3.42)  ‘for 
K1LHN1  and  (3.43)  and  (3.44)  for  KALMH2. 


*  (k)  = 


4  (k)  = 


(3.41) 


(3.42) 


4(k)  * 


1  T  T^2 
0  t  T 
0  0  1 


(3.43) 


(3.44) 

0  and  ^  are  in  general  functions  of  k,  however  for 
this  simulation  they  are  not  since  a  constant  data  rate  is 
assumed  and  no  extended  predictions  are  required.  The  U(k) 
matrix  would  be  utilized  to  include  deterministic  forcing  in 
the  model,  if  this  information  were  available  as  in  [ 1 ]. 


4(k)  = 


T3/S 

T^2 
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since  an  adaptive  Kalaan  identifier  was  not 


K 


ac 

►  ■ 

v. 

l 

I 


< 


However, 

implemented  to  estiaa te  the  pilot/aircraft  response,  no 
atteapt  was  aade  to  include  deter ainistic  forcing  in  the 
aodel. 

1  .  Kalaan  Filter  Covariance  of  Measurement  Noise.  R  j)c) 
Kalaan  filter  theory  assumes  linear  relationships 
aaong  measurements  and  states  as  can  be  seen  froa  (3.9). 
Since  aircraft  motion  is  modeled  in  a  Cartesian  reference 
frame  and  measurements  are  generated  in  a  polar  reference 
frame,  the  resulting  relationships  aaong  the  states  and 
measured  values  are  nonlinear,  as  can  be  seen  from  the 
transformation  equations  shown  below. 

x  =  R  cos(B)  sin  (A)  (3.45) 

y  3  R  cos(E)  cos(A)  (3.46) 

z  =  R  sin  (E)  (3  .47) 

Using  these  polar/Car  tesian  transformations  to 
nor.linearly  combine  the  polar  observations, 

three-dimensional  Cartesian  measurements  are  generated  from 
(3.9)  to  fora  (3.48)  below. 
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Zx  (k) 
Zy  (k) 
**  00 


R  cos  (E)  sin  (A) 
R  cos  (E)  cos  (A) 
R  sin  (B) 


0  vx(k) 

0  +  ▼y  (k) 

Jz(k) 


(3.48) 


where  the  observation  matrix  for  KALHI1  is 


H  (k)  = 


(3.49) 


and  for  KALHH2  is 


H  (k)  = 


ununs 

000000100 


(3.50) 


In  order  to  compute  the  measurement  error  variance 
it  is  necessary  to  first  linearize  the  measurement  error. 
Differentiating  equation  (3.48)  with  respect  to  each  of  the 
measurement  variables  yields  (3.51),  where  s  and  c  represent 
sin  and  cosine  respectively. 


J(x)  = 


sAcE  rcAcE 
cAce  -rsAcE 
SE  0 


-rsAsE 

-rcAsE 

rcE 


(3.51) 


Thus  we  find  that  the  linearized  Cartesian  errors  can  be 
expressed  as 


Therefore  assaying  no  cross  correlation  of  polar 


errors,  the  linearized  Cartesian  measurement  error 

covariance  eatrix  is 

Hc(k)  *  J(x)  Bp  (k)  J(X,T  (3.53) 

The  diagonal  terns  of  R  (k)  are 

R  (1,1)  *  r2  (o|sE2sA2  ♦  o2cB2cA2 )  ♦  o2cE2sA2  (3.54) 

R(2,2)  *  r2  (o|sE2cA2  ♦  c£cB2sA2)  ♦  o|cE2cA2  (3.55) 

R  (3,3)  =*  r2o£cE2  ♦  oJsE2  (3.56) 

The  off-diagonal  elementsare 

R  (1,2)  *  r2o|  sE2sA2  ♦  (o£  -  r2a2  )  cE2sAcA  (3.57) 

R  (1,3)  =  (ajj  -  ra2  )  sEcEsA  (3.58) 

R  (2, 3)  ♦  (a2  -  r^|  )  sEcEcA  (3.59) 

and  due  to  the  symmetry  of  R  (k) 

R  (2,  1)  =  R  (1  ,2)  (3  .60) 

R  (3,  1)  =  R  (1  ,3)  (3.61) 

8  (3,2)  =  R(2,3)  (3.62) 


It  should,  be  noted  that  the  R(k)  matrix  is  not  constant 
since  it  depends  on  range,  azimith,  and  elevation. 

2 .  Filter  Initialization 

All  three  filters  were  initialized  with  reasonable 
state  values  fcr  position,  velocity,  and  acceleration  on  the 


the  first  pass  through  the  filter. 


since  it  is  assumed  that 


the  GOBS  has  maintained  a  good  track  for  sometime  prior  to 
the  final  leg  to  the  target,  consequent!?  the  covariance  of 
error  for  the  initial  state  prediction  vector  is  not  set  to 
an  arbitrarily  large  nuaber  such  as  106,  as  is  often  done 
when  there  is  little  confidence  in  the  initial  state  values. 
Instead  103  is  used  since  it  is  more  consistent  with  the 
covariance  of  error  in  good  initial  state  values.  This 
simulated  pass  from  some  other  tracking  filter  to  the  filter 
cf  interest  is  realistic  and  reduces  the  settling  time. 

Program  constants  and  constant  array  calculations 
for  the  Kalman  H,  0,  At  and  a  matrices  are  set  up  on  this 
first  iteration.  The  process  noise  9  is  set  at  an 
arbitarily  small  number  to  ensure  that  the  gain  matrix  will 
net  converge  to  zero  and  accentuate  divergence  problems. 
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A.  QUALITATIVE  OBSERVATIONS 

Seven  different  filter  conf igurations  were  ultimately 
implemented  and  evaluated  with  the  GDBS  simulation.  These 
configurations  are  discussed  below  and  assigned 
alpha-numeric  symbols  for  ease  of  reference. 

The  sixth  order  alpha-beta  filter,  ALPBTA(AB)  was 
implemented  with  the  simple  parameter  switching  technique 
outlined  in  Chapter  3.  However,  the  heading  rate  maneuver 
detection  process  proved  to  be  too  insensitive  to  slow 
maneuvers  and  was  therefore  augmented  with  a  trigger  that 
reset  filter  parameters  when  the  heading  error  exceeded  3 
degrees.  The  results  of  this  change  proved  to  be 
worthwhile,  as  will  be  shown  later. 

The  next  three  filters  are  variations  of  the  sixrh  order 
KALMN1  filter.  (K10)  is  KALMN1  without  adaptation,  (K11)  is 
KALMN1  with  the  process  noise  adaptation  scheme  fcr  modifing 
Q(k),  and  (K12)  is  KALMN1  adapted  by  resetting  P(k),  the 
error  covariance  marrix,  as  discussed  in  Chapter  3.  Filters 
(K20)  ,  (K 2 1 ) ,  and  (K22)  are  variations  of  KALMN2  which 
correspond  to  the  KALMN1  variants  described  above. 


Since  the  objective  vas  to  evaluate  the  accuracy  and 
response  of  each  filter  in  the  final  phase  of  boeb  delivery, 
the  siaulation  was  initialized  with  the  aircraft  within  90 
seconds  of  the  release  point,  at  a  speed  of  480  knots,  and 
on  a  heading  within  10  degrees  of  the  target  bearing.  For 
each  run,  after  allowing  5  seconds  for  the  filter  to  settle, 
Tacan  target  bearing  information  was  provided  to  the  pilot 
controller.  At  15  seconds  elapsed  tiae,  filter  adaptation 
was  enabled  and  random  maneuvers  were  begun  at  20  seconds. 
Five  separate  runs  were  evaluated  for  each  filter  where  the 
random  maneuvers  were  ceased  at  80,  60,  50,  40,  and  30 

seconds  prior  to  arrival  at  the  release  point. 

The  following  plots  were  generated  for  the  case  where 
maneuvers  were  stopped  with  a  time-to-go  of  50  seconds. 
Figures  4.1  through  4.21  show  true  and  estimated  (connected 
symbols)  position,  velocity,  and  acceleration  trajectories 
as  functions  of  time  for  all  filter  configurations.  Note 
that  the  target  position  is  designated  by  a  circle  on  the 
horizontal  trajectory  plot.  Also  representative  filter  gain 
schedules  are  included  to  show  the  effects  of  the  particular 
adaptation  process  being  utilized. 
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Figures  4.1  and  4.2  indicate  the  magnitude  of  the 
maneuver  encountered  and  the  good  state  estimation  qualities 
of  this  sieple  sixth  order  alpha-beta  filter.  Notice  there 
is  no  significant  divergence  of  the  estimated  trajectory 
froe  the  true  trajectory  throughout  the  run.  Figure  4.3 
shows  the  step  gain  adaptation  at  the  beginning  of  the  run 
in  response  to  the  controlled  turn  to  the  target  heading. 
This  gain  is  then  reduced  after  the  turn  is  coapleted  and 
before  the  first  randoa  maneuver  begins,  when  the  gain  is 
again  increased. 

The  trajectory  shown  in  Figure  4.4  contests  sharply  with 
that  shown  in  4.1,  showing  significant  filter  divergence  for 
the  nonadaptive  sixth  order  Kalman  filter.  The  significant 
lag  in  velocity  state  estimation  shown  in  Figure  4.5  results 
from  the  convergent  gain  properties  characterized  in 
Figure  4.6. 

The  performance  of  K10  changes  dramatically  when  it  is 
made  adaptive  as  shown  in  Figures  4.7,  4.9,  and  4.9  for  K11 
and  Figures  4.10,  4.11,  and  4.12  for  K12.  Figure  4.9  shows 
continuous  gain  adjustment  in  response  to  perceived  changes 
in  the  process  noise.  Figure  4.12  shows  the  effect  of 
resetting  the  covariance  of  error  in  response  to  a  maneuver. 


Pigures  4. 13, and  4.14  show  the  improvement  in  state 
estimation  of  the  K20  nonadaptive  Kalman  filter  .when  the 
order  is  increased  over  that  of  K10.  It  is  interesting  to 
note  that  we  see  significant  overshoot  in  the  velocity 
estimate  for  the  first  time.  Unlike  the  filters  discussed 
thus  far,  K20  provides  an  acceleration  estimate  which  can  be 
seen  in  Figure  4.15  and  accounts  for  the  sensitivity  of  the 
velocity  estimate. 

Adapting  K20  through  the  noise  process  technique  results 
in  K21  which  produces  the  position,  velocity,  and 
acceleration  estimates  shown  in  Figures  4.16,  4.17  and  4.18 
respectively.  Figures  4.19,  4.20,  and  4.21  provide  the  same 
information  for  K22,  which  represents  the  covariance  of 
error  adaptation  variant  of  K20.  These  last  two  adaptive 
filters  show  little,  if  any,  apparent  improvement  over  the 
nonadaptive  version.  This  observation  is  supported  in  the 
following  section. 

3.  QUANTITATIVE  RESULTS 

A  single  run,  for  each  filter  configuration  evaluated 
for  each  maneuver  termination  time,  is  not  sufficient  to 
properly  determine  filter  performance  over  the  ranqe  of 
possible  maneuver  trajectories  and  measurement  noise 


43 


sequences.  Therefore,  30  simulation  trajectories  per 
filter,  per  Maneuver  period,  were  conducted  with  different 
randoa  maneuver  and  aeasureaent  noises  sequences  generated 
for  each  run.  The  boab  release  signal  to  the  pilot  was 
assuaed  to  occur  at  the  closest  point  of  approach  (CPA)  to 
the  tarqet  release  point.  The  average  of  the  resulting 
CPA's  for  each  30  test  runs  are  shown  in  Table  I.  CPA's 
greater  than  250  feat  are  classified  unsatisfactory  and 
labled  'O'  appropriately. 

At  a  glance  it  is  apparent  that  the  adaptive  sixth  order 
alpha-beta  filter  performs  very  well  in  such  a  dynamic 
environment,  except  when  maneuvers  are  continued  very  close 
to  the  tarqet.  The  nonadaptive  sixth  order  Kalman  filter  is 
obviously  unsuited  by  itself,  but  when  made  adaptive, 
preforms  very  well,  particularly  for  the  process  noise 
adaptation  technique  when  maneuvers  are  terminated  late  in 
the  target  run.  The  ninth  order  Kalman  filter  performance, 
both  adaptive  and  nonadaptive,  is  comparable  to  the 
alpha-beta  and  adaptive  sixth  order  Kalman  filters,  but  has 
problems  in  close  due  to  its  longer  settling  time.  Notice 
that  the  adaptive  variants  of  the  ninth  order  Kalman  have 
little  effect  or.  that  filter's  performance,  as  we  surmised 


in  the  last  section 
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Table  II 
and  memory. 


shows  the  relative  cost,  in  computation  time 
to  implement  KALMN1  and  KALMN2  in  relation  to 


ALFBTA . 


50 


TABLE  II 


Relative  Computation  and  Heaory  Costs 


Alpha-Beta  (sixth  ord) 

1 

Kalman  (sixth  ord) 

2 . 9 

Kalman  (ninth  ord) 

3 . 4 

The  only  advantage  to  i mplementing  the  most  responsive 
variant  of  K&LMN1  would  be  to  reduce  the  probability  of  a 
GOBS  generated  abort,  due  to  large  predicted  bomb  impact 
errors,  when  maneuvers  are  carried  very  close  to  the  release 
point.  Lastly,  if  the  adaptive  Kalman  identifier  proved  to 
be  useful  in  providing  deterministic  forcing  for  the  Kalman 
filter  model  and  resulted  in  improved  accuracy  and  response 
over  the  alternatives  presented,  the  cost  in  computation  and 
memory  resources  would  be  even  greater  than  we  have  seen 
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Pig.  4.9.  K&LflHI (K11)  T-Gain  Schedule 
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Pig.  4.17.  KALMH2  ( K21)  True  and  Estimated  Y-Velocity 
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Fig.  4.18.  KALMN2(K2  1)  True  and  Estimated  T-iccelerati on 


Fig.  4.19.  K&LHN2(K22)  Horizontal  Position  Trajectory 
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*******  *ground  oirecteo  bombing  system  simulation********** 


IMPORTANT  VARIABLES 
TACRRXC  STATE,  TIME . 
TACRRY( STATE, TIME  I 
TACRPZ! STATE, TIM* 


??FI¥S8! 


TACACX! 
TACACY! 
TACACZ! 
TACREX! 
TACREY! 
TACREZ! 
TJCRMX 


.  M 

RMY  -  M 


A/C  STATE!  l»PO$IT,2«VEL, 
EL)  a  TIME(N)  IN  IADAR 


nmi? »I!«S ^cooro 

1st.  ai£crafth1tatei ",*•,■)  at 

TIME! N)  IN  A/C  CARTESIAN  COORD 
, .  REFERENCE  FRAME. 

. . _JMB  bi  HBHI8BWI  H  §85: 

TACRMZ  -  MEASURED  A/C  POSITICN(Z)  IN  ROR.  FRAME 
RDSFQ  -  RADAR  DATA  SAMPLE  FREQUENCY. 

DT  -  RADAR  SAMPLE  INTERVAL  TIME. 

N  -  N-TH  ITERATION  AT  SAMPLE  RATE  DT. 

TS85JI2 :  Isif  ^oSSWEtMe* 

TTS-^-TIMS JfdS n$TMn*BI*dgt  and  tachog.  ne 
ELAPSE  TIME  IN  SECONDS  SINCE  BEGINNING  OF 


HDGERR 

ELPSTM 


SRTMVR 
ST  PM  VP 
MNVRPT 
CNTLPT 


SIMULATION. 
IT  MANEU 


!  MANEUVERS! A/C  MANEUVERING  CAN  BEGIN. 
MANEUVERS! i/C  MANEUVERING  MUST  STOP. 


_TAR 

STOP  _ _ 

MANEUVERING  PILOT 
-  CCNTROLED  PILOT. 

MODEL  -  FRjj|c|gERTIA  M00EL  0F  A/C  WITHCUT  WINP 

NTIM^l  -  NuSSiR^OFBT?M§SGVHftu  SIMULATION  LOOP. 
GTRflG  -  RANGE (RNG)  FROM  A/C  TO  TGT  ALONG  DESIRED 
GRCUNO  TRACK. 


TGTRRX 

TGTRRY 

TGTRRZ 


-  TARGET  COORD.  IN  RDR  FRAME 


TWDRR  -  TRUE  WIND  DIRECTION  IN  RADAR  FRAME 
TWVRR  -  TRUE  WIND  VELOCiTY  IN  RAOAR  FRAME 
TWDRP  -  5ST.  WIND  DIRECTION  IN  RAOAR  FRAME 
EST.  WIND  VELOCITY  IN  RADAR  FRAME 
-  TRUE  WIND  X-Y  VELOCITY  COMPONENTS 


-  EST.  WIND  X-Y  VELOCITY  COMPONENTS 

-  PROCESS  NOISE  VARIANCE 


TWVRR  - 
TWVRRXf 
TWVRPY 
EWVRPX 
EWVRRY 
X2SIGM 
Y2S IGMI 
Z2SIGM* 

PINITL  -  ERROR  COVARIANCE  DIAGONAL  INITIAL  VALUE 
GX  I 

GY  -  FILTER  X,Y,Z  GAIN  VALUES  AT  TIME  N 
GZl 

YRp  -  FILTER  X,Y,Z  RESIOUE  VALUES  AT  TIME  N 
ISdIeS  -  RAOIAL  RESIDU  AT  TIME  N 


IMPLICIT  REAL! A-H,C-Z) , INTEGER! I-N) 


nnnonn  nnno  nnnon 


OOU8LE  PPECISICN  OS 
COMMON/ OOUBLE/CSEED 
CONMON/TMRRST/NtTAC 
►3,1000) _ 


CSEED 

ACRRXf  3,1000)  •  TACRRYO  *1000 )  , TACRRZ ( 

^  .  _  *m|  J4  / T  M  AC  ST  /NT  *  T  AC  ACX  (  3  •  1 000  )  ,  T  AC  ACV<  3,1000),  T  AC  AC  LI 

SONMON/TGTWNO/TGTRRX ,T  GTRR Y , TGTRRZ, T  MORR , TWVRR , EWDRR • 
♦EWVRR 


E3mm8n^  T  feJSx  ( lo6oIi?  acjImy  ( 1000  *  f  T  ACRMZ  (  1000 

COMMON/FILTSR/TACREX(3,1050),TACR£Y(3,IOOO)  ,TACREZ 


) 

Z  €  3 1 


♦1000) 

^888?^??SS§§^!??86Sf?i^Ilfi§85f,;XSgli!gS§?Y^GX< 

♦»YRES (10001 ,ZRES (1000) • Mil ( 1000 J ,M22( 1000) ,W33( 1000) * 
*Pi 1000*9) »PIN I TL 

S?M?8si ON  xlfloSS?? lui&8)?TGTX(  1 ),  TGTY(  1 )  ,TRUACC<  1000) 
♦,TIME( 1000) ,X1( 1000) ,Y1( 1000), ESTACC (1000) 


INITIALIZE  PARAMETERS; 

BNKANG-0.0 

OEADZN-O.O 

GLDHOE-O.O 

K»0 

Kl-0 

K2-1 

TB-2.G 

PINITL-1000. 

SRTMVR-20.C 

STPMVR-50.0 

NTIMES-800 
SECOND-0. 0 
HOGERR-O.O 
G» 32. 174049 
G2-16. 087007 
GKl-3.5 
GK2-3.5 

PI *3. 141592654 
OLDHOE-O.O 
OSEED-  1.0C0 
RSfGMA-100. 

ASTGMA-.00 1 

ESIGMA-.001 

KFILTR-0 

KADAPT»0 

X2SIGM-1. 

Y2SIGM-1. 

Z2S I GM-1. 

READ  INITIAL  AIRCRAFT  STATES  IN  AC  REF.  FRAME. 
WR ITE ( 6  » 6 ) 

REAO ( 5 » 1)  TACACX(l,ll,TACACY(l,ll,TACACZ(l,l) 
REA0(5« 1)  T  AC ACX (2,1),  TACACY (2,1),TACACZ( 2,1) 
REAO( 5,1)  TAC ACXl 3 , 1 ) , T  ACACYt  3 ,1),TACACZ(3,1) 


OBTAIN  X  £  Y  MIND  COMPONENTS  IN  RADAR  FRAME  £  ADD  TO 
CORRESPONDING  A/C  INITIAL  VELOCITY  STATES  TO  08TAIN 
INITIAL  AIRCRAFT  STATES  IN  RADAR  REFERENCE  FRAME. 
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3 

41 

C 


C 

C 


C 

c 

c 

c 

c 


c 

c 

c 


45 

46 
59 


READ<5,2)  TGTRRX,TGTRRY»TGTRRZ»  TWORRtTHVRR,EWORR  »EWVRR 
TWVRRX-TWVRR*SIN<TWDRR*PI/180.0)*l. 687805556 
TWVRRY-TWVRR*CaS<TW0RR*P!/|8Q.01*i.687805556 
EWVRRX«EWVRR*SIN(EWORR*P 1/180. 0>*1 .687805556 
EWVRRY-EWVRR*C0S(EWDRR*PI/180.0)*1. 687805556 

00  n  I"1:,3 

TACRF 

TACRI  _ 

TACRRZ< 2,1)»TACACZ(2,1) 

GO  TO  41 
CONT INUf 


:  m\ W 


TACRRXl 1 *1 )»TACACXC 1 .1) 
TACRRYi 1 ,1 )«TACACY( 1,1) 
TACRRZUtl  J-TACACZI  1,1) 
CONTINUE 


WRITE  6 
WRITE (6 
WRITE  6 
WRITE (6 
WRITE (6 
WR  IT*  6 
WRITE (6 
WR  IT£  (6 
WR!tE(6 
WRITE <6 
WRITE <6 
WRITE <6 
WRIT= (6 
WRITE  6 
WRITE  (6 
WRITS  6 
WRITE (6 
WRITE (6 


4) 

II 

iii 

Ki 

z8i 

21) 

22) 

24) 

io! 

53) 

60) 

63) 

65) 


rACRRXIl. 1) • TACRRYi It  1) , TACRRZ < 1 » 1 ) 

T ACRRX 12  til* TACRRY < 2 » 1 ) * TACRRZC 2,1 ) 
TWORRtTWVRR 
EWCRR, EWVRR 

fk  «  A  .4  A  A  I 


R  SIGMA ,ASI  GMA»ESIGMA 

pInJ?l,Y2SIGM,Z2SIGM 


KFILTR 
K ADAPT 
SRTMVR 
STPMVR 


DETERMINE  RAOAR  SAMPLE  I NTERVAL ( CT) . 

DT*1.0/FL0AT< IRDSFQ) 

PERFORM  SIMULATION  LOOP  N-TIMES. 

DO  900  N  *  I*  NTlMES 
NT*N 

ELPSTM  «  IN-1)  *  OT 
TIME (N1*ELPSTM 

NOW  CALL  PLPNCI  TO  TRANSFORM  RADAR  CARTESIAN  COORDS 
IN  POLAR,  ADO  WHITE  NOISE,  AND  RETRANSFORM  INTO  ROR 
CARTESIAN  CQOROS  IN  PREPARATION  FOR  FILTERING. 

CALL  PLRNO I (R S IGMA, ASIGMA, ES IGMA, RANGE, AZMI TH, 
♦ELEVTN) 


IF  ( KFILTR  .NE.  0)  GO  TO  45 
CALL  ALFBTAIDT ,HQGERR»TTG) 

GO  TO  59 
CONTINUE 

IF  (KFILTR  .NE.  1)  GO  TO  46 

CALL  KALMNKKAOAPT,  DT,RSIGMA,  ASIGMA, ES  IGMA, RANGE, 
♦AZMITH, ELEVTN, TTG) 

GO  TO  59 
CONTINUE 

CALL  KALMN2 (KADAPT, DT,RS I GMA , ASI GMA, ES IGMA, RANGE, 
+AZMITH,cLSVTN,TTG) 

CONTINUE 
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8 

C 


C 

c 


ftfOW  CALL  TACAN  SUBROUTINE  TO  OBTAIN  HEAOING  TO  FLY 
CALL  TACAN ( TACHOG, OSROGT • TAC VEL » TTG » GTRNG » HOGERR J 


IP  | N  *GTi .aOOJ.GO 


580 


♦TACRRZI 

♦TACRRXi 


F  iM00(N^i;8)'.N?.!  01  GO  TO  550 


♦l.Nl.TACREX 
♦  TACREXO* 


3,N) tTACRRY (3 ,N), TACRRZI 3,N) 
WRITE(6,78)  T ACREX ( 1 »  N)  •  T A 


WRf 


♦  » HOGERR  t  DSR 
MR 


<: 


TACREY(1,N) »TACREZ< 
"  2,N), 

*NI 


TE(6,78)  TTGtTACRMXl N) , TACRMY(N) , TACRMZ(N) 
8)  G&xf  N^is^f  N^«^Z(  N  ),XRES(N) ,  YRESCNJ 


550 

580 


Et6,78)G&X(N _ 

♦, ZRES(N 1 , Wll( N) , W22( Nl ,W33CN) 

WRITE (6,78)  P(N»l),P(N,2)fP(Nf3),P(N,4), 
♦P(N,5),P(N,6) »P<  N,  7) , P (N, 8 ) ,P(N,9) 

,teINul 


CONI 


NO  CONTROL  INPUT 
IF  ( ELPSTM  .LT.  5 


untjl  FILTER  SETTLES. 


G  TO  800 
DETERMINE  CONTROL  INPUT  TO  AC. 

NO  CONTROL  INPUT  IF  TTG  IS  LESS  THAN  3  SECOND  OR  IF 
CLOSEST  POINT  OF  APPROACHICPA  J  HAS  BEEN  RE  ACHE  ft. 

CALL  TTGCPA(N»K, TTG, GTRNG, I FLAG) 


♦  )> 


IP 

GO 


WlWa> 


.EG.  1)  .AND.  4 ABS( HCGERR J  .GT.  DEADZN 


GO 


T  ACACXI 3 ,N) *0. 0 
T ACACYi 3  »N) *0. 0 
T  ACACZ4  3 ,N )*0 .0 
TO  800 


CHOOSE  MNVRPT  OR  CNTLPT  RESPONSE  BASED  ON 
MAGNITUDE  GF  HOGERR, AND  SRTMVR  AND  STPMVR  CRITERIA. 

600  CONTINUE 

IF  ( (ABS(HDGERR) )  .GT.  .523598776)  GO  TO  700 

IF  << ELPSTM  .LT.  SRTMVR). OR.  (TTG  .LT.  STPMVR)) 
♦GO  TO  700 

CALL  MNVRPT(Kl) 

GO  TO  SCO 

CALL  CNTLPT  SUBROUTINE  IF  MANEUVER  NOT  PERMITTED. 


700 

800 


C 


CONTINUE 

CALL  CNTLPT (K2»HDGERR) 

PROVIOE  MODEL  SUBROUTINE  AC  STATES  AT  TIME  N. 
CALL  XMODEL 

MOOEL  RETURNS  STATES  IN  AC  FRAME  FOR  TIME  N+l. 

ADO  WIND  EFFECTS  OVER  DT  TO  OBTAIN  AC  STATES  IN 
RADAR  FRAME. 

CALL  WNDEFFITWVRRX, TWVRRYtDT ) 


i?NTINUS 
FORMAT! 3F10.0) 


FORM AT (7F10.0 ) 
FORMAT (58X,*GC 


SIMULATION* ) 


RSttf I ^<^S>CZ«lHfa.CraBlTUBSiA 9RAOAR  REFERENCE 
+FOFMlTi5x!*A/i3lNITiAL°viLoilTY  IN  RAOAR  REFERENCE 

FORMAT !5X*  • ESt •  WIND  * ' . 10X.F10. 3. ' / ' . 1X.F9. 3//) 
FORMAT! 5X, 'MEASUREMENT  NOISE  VARIANCES!R!FT) , A!RAD) , 

^FORMA^f  Isx*  iMoCE§S^NQIS£  ^VARIANCES!  X,  Y,Z  )  *»,16X, 
♦3!10X,FlC.3i/i 

FORMAT! 5 X, 'ERROR  COVARIANCE  DIAGONAL  *•  * 10X,F10.3//I 
FORMAT!5X, 'FILTER  DESIGNATION  *',4X,I1/I 


♦•TACRRZl*  *6X»  'TACRRX2*  ,6X, 'TACRRY2*  ,oa,  -  ,oa, 

♦  'TACRRX3'  ,6X,  'TACRRY3*  ,6X, ' TACRRZ3* I 

FORMAT! *0* » loXt'TACREXl' »6X, 'TACREYI • »6X* *TACREZ1* *6X 

♦  *' TACREX2* »6X» 'TACREY2' »6X» ' TACREZ2* ,6X, 

*'T ACREX3*  ,6X» 'TACREY3* , 6X, • TACREZ3* ) 

FORMAT! *0*  ,22X,'TTG' ,7X,*TACRMX* ,7X, 'TACRMY* ,7X, 

♦  •TACRMZ*  f7X,'  FOGERR' ,7X,'DSRDGT«  ,7X, 


6X**  TACRRY1*  » 6X» 
6Xf  *  TACRRZ2'  ,6X, 


•TACRMY*  ,7X, 


7X» ' Z— GAIN'  i 


PKK88 1 


♦  *T ACHDG* »7X»  • TACVEL* *8X* 'GTRNG* » 

FORMAT  (•  O'  ,  19X,  ' X-GA IN ' »7X*  *  Y-GAIN* » 7X* ' Z-GAIN' » 5X 
♦•X-RESIDU*  ,5X,*Y-RESIDU' ,5X, * Z-RESI OU' ,7X, • W! 1 ,1 ) ' 
+,7X, *W! 2,21 *, 7X, 'W! 3,3) •) 

FORMAT!  'O'  ,20X,*PKKU'  ,  8X,  *  PKK22  •  »8X*  "»KK33*  ,  8X, 
♦'PKK44*  » 8X, *  PKK55 ' ,8X, 'PKK66' »  8X  ,' PKK77* ,8X,*PKK88 
♦,8X,»PKK99'///) 

FORMAT! ///10F13.4) 

FORMAT! 13X.9F13.4) 

ENO 

SUBROUTINE  TACAN! TACHDG, DSRDGT , TACVEL ,TTG, GTRNG, 
♦HOGERR) 

IMPLICIT  REAL!A-H,0-Zi, INTEGER! I-N) 

DOUBLE  PRECISION  DSEED 
COMMCN/OOUBLE/OSEED 

COMMCN/TMRRST/N ,T  ACRRX! 3 ,1000 i ,TACRRY (3,1000), 
♦TACRRZ ( 3 , 1000 ) 

COMMCN/TGT*ND/TGTRRX,TGTRRY,TGTRRZ,TWDRR,TWVRR, 
♦gWDRR ,EWVRR 

COMMON/ P ILCT/T 8, SRTMVR, STPMVR , OT 

COMMON/ FI LTER/TACREX(3,1000) ,TACREY(3, 1000 ) , 

+tacc^mcA>?a8  AMT/G,G2, GK1,GK2 


*TACgSS&g* 


»!*3. 141592654 
SMO*  EWDRR*  P 1/ 180.0 
EM V*EMVRR* 1.68780556 

TACVEL* SORT (T  ACREX! 2  »N)*TACRE  X(2  *  N)+TACREY! 2,N)* 
♦TACREY! 2»N) ) 

TACHOG*TRUHOG! TACREX! 2, N) »TACREY! 2  »NI ) 
TGTDIR*TRUFCG! ( TGTRRX-TACREX! 1 ,N J ) , (TGTRRY- 

♦tacreyTi.n) n 
OSROGT-TGTCIP 
HOGERRaCSROGT-TACHOG 

GTRNG*SQRT<! TACREX! 1 ,N)-TGTRRX)**2+( TACREY ( I,N )- 
♦TGTRRY ) **2 ) 

TTG* GTRNG/ TACVEL 


77 


noon  non  non 


END 


RETURN 


600 


700 


800 

900 


SUBROUTINE  TTGCPA(N,K,TTG,RNG* I  FLAG) 

IF  (  (TTG  .GT.  3.0)  .OR.  (K  .EQ.  0))  GO  TO  600 
GO  TO  700 
CONTINUE 

I?  (N  .LT.  100)  OLDRNG-RNG 
DIFFNCOLDRNG-RNG 
IF  (TTG  .GT.  10.)  GO  TO  800 
IF  (DIFFNC)  700*800*800 
IFLAG  —  1 
K«i 

GO  TO  9C0 
IFIAG*1 
CONTINUE 
OLDRNG»RNG 
RETURN 

ENO 


SUBROUTINE  MNVRPT(Kl) 

DOUBLE  PRECISION  DSEEC 
CONHON/OOUBLE/OSEEO 

CONMON/TMACST/N*  TACACXO*  1000  )» TACACY <  3*  1000)  * 


♦  TACACZ(3.,JOOO^t 


90 


CnMHON/PlLOT/TB»SRT  MVR*  STPMVR  »OT 
COMMON/PARAMT/G* G2*  GK1.GK2 
IF  ( ( N— K1 )  .EQ.  1)  GO  TO  100 
CMOACC-O.O 
N1*0 

PA*GGUBFS(  OSEED) 


100 


AMXACC»14.0*(PA-.5) 
ACCDUR-20.0*EXP( .25* ABS ( AMXACC ) ) 


END 


CONTINUE 
ACCOUR-ACCCUR-OT 
IF  ( ACC OUR  .LE.  0.0)  GO  TO  90 

CMCACC*AMXACC*( l.-EXPC— OT/TB) )+CMDACC*EXP ( -OT/TB ) 
AACHOG*TRUHDG(T AC  ACX  ( 2«  Ni .TACACY ( 2«N) ) 

TACACXI 3  »N ) «CMOACC*CCS<  AACHDG )*G 

TACACY!  3,N )»— CMDACC*S IN ( AACHDG)*G 

TACACZ(3*10CO)*C.O 

K1»N 

RETURN 


SUBROUTINE  CNTLPTC K2 .HOGERR ) 

GENERATE  CCNTROLED  PILOT  BANKANGLE. 
PH01  JS  £0MP0NgNT  BA§gO  ON  ANC  "  ““ 


PH02  IS  COMPONENT  8AJ 


. . 3LE  ERROR  HOE. 

,  _  ON  ANGLE  ERROR  RATS  hOEOOT 

»HD  IS  OESIPED  PILOT  GENERATEC  BANK  ANGLE. 

DOUBLE  PRECISION  DSEEO 
COMMON/DCUBLE/CSEED 
COMMON/TMACST/N  »TAC> 

♦  TACAC2j(3*1000) 


iTACACX(3*1000)*  TACACY (3*1000)  , 
'Co6Mofi/PARAMT/GiG2,GKl,GK2 

i  U  M  &  a  J  M  •*  4  «  ^  n  V  4J  1  i  M  ^  *  n  > 


f2Mrrac^i!’!R;t^taTioT’oT 


OLOHDS 
BNKANG 
N  2*0 

CONTINUE 


>G.O 

‘C.O 


200 


on  on 


HOep^Tvt  HDGERR-GLDHCE  )/DT 
PHOl*GKl*HOGERR 
PHQ|«GK2*H0ED0T 
CMDANG-PHD1+PHD2 


IF  }AFflfiB8^GRiGi?T:5|iIt5,cS8ANTg.!?!36 

IF  ICMDANG  .LE.  >.5236)  CMDANG*-.52: 

C0Nf?Nui4 

IF  ICMOANG  .GE.  1.047)  CM0ANG*1.047 

IF  ICMOANG  .LE.  -1.047)  CMOANG*-1.0 
CONTINUE  _  _ 


5236 


CONTINU 

BNKANG* 


•  1.047) 

.  -1.047) 


CM0ANG*1.047 
CMOANG*- 1.047 


MDANG*(  l.-eXPI-OT/TBJ  )+BNKANG*EXP<  -OT/TB) 
QRT(TACACX(2«N)*TACACX(2tNi+TACACY(2,N)* 


AACVEL*SQRTITACACX(2,N)*TACACXt2,N)VrA 
♦  TACACYI  2.N) ) 

AACHCG*TRUHDG I TACACXI 2»N) »TACACYI 2  »N) ) 

HOG  '  AT*TAN ( BNKANG )*G/AAC VEL 

TACACXI3  »N )*AAC VEL*HD6EAT*CQS (AACHCG) 

TACACYI3,N)*-AACVEL*HDGRAT*SINIAACH)G) 

TACACZI 3 ,N ) *0 .0 

3L0H0E-H0GERR 

K2*N 

RETURN 

ENO 


SUBROUTINE  XMOOEL 

DOUBLE  PRECISION  OSEED 
COMMCN/OOUeLE/ OSEED 

C0MMCN/TMACST/N,TACACXI3 ,1000 ).TACACY(3t 1000) , 
+TACACZC3* 1000 ) 

COMMON/ PILCT/TB »  SRTMVR»STPMVR,DT 
C0MM0N/PR0CES/X2SIGM » Y2S IGM*Z2SIGM 


S?M®0i5RJS^^if:g^l^i),X<6.1i.U43,l) 

♦XN1 16, 1),X 116,1) 

T2*(CT*DT)/2 

IP  COMPUTE  Ha?E  TRANS?TION  MATRIX(PHI). 
INITIALIZE  MATRIX  a  0.0  t  1.0  ON  OIAGO 
00  130  1*1,6 
00  120  J*1 , 6 
PHI! I , J) *0.0 

IF  <  I  .EG.  J )  PHI ( I , J) *  1.0 
CONTINUE 
CONTINUE 
PHI ( 1, 2 )*0T 
PHI C  3 ,4 ) *0T 
PHI (5,6 )=DT 

COMPUTE  OEL  MATR  IXCDEL ) 

INITIALIZE  a  0.0. 

00  150  1*1,6 
DO  14C  J* 1, 3 
DEL(I,J)*0.0 
CONTINUE 
CONTINUE 
OEL( 1 , 1 ) *T2 
DELI  3, 2 )*T2 

DeL<4,2  )*DT 
DELI  6,3 ) *DT 


MATRIXIPHI). 

1.0  ON  01  AGONAL. 


PHI  1 1 , J) 1 


S ET  UP  'X*  MATRIX 


non  non  no  nnnnn  o  no  noon  on  non 


160 


X(  1»  1  )*TAC  ACXC  1,  N ) 

X<2»l)«TACACX(2»Ni 
X(3,1)*TACACY< 1,N) 

X(4»1)*TACACY(2»N) 

X< 5»1)*TACACZ( 1»N) 

X16,  U*TACACZ(2,N) 

CALL  PRCOUCT  SUBROUTINE  TO  MULTIPLY  PHI  £  X 
CALL  PRCCCT<PHI,X,6,1,6,X1) 

SET  UP  'U*  MATRIX. 

U(3,1)*TACACZ(3,N) 

CALL  PRODUCT  SUBROUTINE  TO  MULTIPLY  DEL  £  U 
CALL  PRC0CT(0EL«U*3tl«6fF) 

CALL  AOD  SUBROUTINE  TO  A00  X  £  F  TO  OBTAIN 
STATES  AT  TIME 

CALL  A004XlfFtl»6fXNl) 


IP*N+1 
TACACXI 1 
T  ACACXi 2 
TACACXC3 
TACACY4 1 
TACACYC2 
TACACYJ3 
TACACZl  1 
T ACACZ4  2 
TACACZ43 

RETURN 


•  IP)* 

•  IP) 

•  IP) 

:5Si 
:  Si 
:  Si 


!XN1  <  1  •  1 ) 
‘Ta£a£x(3,N) 

!T  ACACY(3  •  N) 
:XN1(  5*1) 

=  XN1  (6,1) 

!T ACACZ(3  ,N) 


THIS  SUBROUTINE  COMPUTES  THE  MATRIX 
STORES  RESULT  IN  C. 

SUBROUTINE  PRCCCT { A, B, N,Mf L • C ) 
DIMENSION  A(L,N)  ,B(N,MJ  ,C(L,M) 

INITIALIZE  *C»  MATRIX3  0.0. 


PROOUCT  A *8 


INITIALIZE  *C»  MATRIX3  0.0. 

00  180  1*1 ,L 
00  170  J*l » M 
C<I,J)-0.0 
CONTINUE 
CONTINUE 

MULTIPLY. 

DO  21Q  1*1, L 
CO  200  J  =  1,N 
DC  190  K*UM 

C< I,K)*C(I,K)+A(I,J)*8( J,K) 
CONTINUE 
CONTINUE 
CONTINUE 
RETURN 


'•I 


nnnnnno  non  non  non 


SUBROUTINE  AOO(A,B,L ,M,C) 

DIMENSION  A(MfL),8(M,U,C(M,L 
INITIALIZE  *C*  a  0.0. 

00  215  1*1, M 
00  213  J*1.L„ 

Cl I, J)*0.0 
CONTINUE 
CONTINUE 

NOW  ADO  'A*  £  •  8*  • 

00  230  1*1, M 

00  ???, J)iitl»JI*B(I,J) 
CONTINUE 
CONTINUE 
RETURN 


SUBROUT INE  WNCEFF ( TWVRRX tTWVRRY  * 0T ) 

COMMON/TMACST/NT,TACACX<3, 1000*, TACACYC3, 1000) , 
♦  TACACZI  3,1000) 

COMMON/ TMRRST/N* TACRRX! 3, 1000 )« TACRRYI 3, LOGO) , 
♦TACRRZI3.10Q0) 

IP*N+I 

TACRRX I 1 ,  I P  )*TACACX  ( 1 , 1 P  i  +DT*TWVRRX*N 
TACRRX!  2,1 Pi*TACACX ( 2 ,1 P i ♦TWVRRX 
TACRRX I  3 , 1 P )*TACACX1 3, 1 P ) 

TACRRYI 1 ,! P  )*TACA*Y ( 1* IP  * ♦DT*TWVRRY*N 


TACRRYll,! P)*TACACY| 1, IP  * ♦DT*TWVf 
TACRRYI  2,1  P)»TACACY(  2, IP )+TWVRRY 
T ACRRYI 3 , 1 P l*TACACY I  3, IP  * 

TACRR  Z ( 1 , 1 P  )*TACACZ 1 1 , 1 P ) 
TACRRZI2,IP  )*TACACZ l 2 » IP ) 

T ACRRZ 1 3 , IP  **TACACZI 3, IP i 
RETURN 


FUNCTION  TRUHCGI XVEL » YVELI 
PI *3. 141592654 
IF  I XVEL*  31,41,51 
IF  ( YVEL )  71,111,71 
I*  (YVEL)  101,91,121 
IF  C  YVEL )  61,91,61  , 

TRUHDG*P  1/2.0  -  A TAN  I YVEL/ XVEL) 

GO  TO  131 

TRUH0G*3.0*PI/2.G  -  ATANI YVEL/XVEL) 
GO  TO  131 
TRUH0G=PI/2.0 
GO  TO  131 
TRUHDG*  P I 
GO  TO  121 
TRUHDG* 3 .O^P 1/2.0 
GO  TO  131 
TRUHDG*0.0 
RETURN 


THIS  SUBROUTINE  TRANSFORMS  CARTESIAN  COORDS  INTO 
POLAR  IN  THE  RCR  REF.  FRAME  THEN  ADOS  WHITE  NOISE  OF 
0  MEAN  AND  VARIANCE,  RS IGMA, AS  I GMA,  AND  ESIGMA  TO  THE 
RANGE,  AZIMITH,  AMD  ELEVATION  RESPECTIVELY.  THESE 


no  o  o  oonn  nnn  nnnnn 


CONSTITUTE  THE  MEASURED  POSIT  OF  THE  A/C,  THESE  NOISY 

aut  TPViiB«s9WRor>ffi.ftsRNe" 


SUBROUT INE  PLRNOI 4RS IGMA,AS IGMA* ES IGMA, RNGPNS , AZI PNS , 
♦ELVPNS) 

DOUBLE  PRECISION  QSEED 
COMMON/DOUBLE/DSEED 

COMMQN/TMRRST/N*TACRRX( 3, 1000) »TACRRY (3,1000), 
+TACRRZ4  3,1000) 

C0MM0N/N0ISE/TACRMX4 1000), TACRMY( 1000), TACRMZ4 1000) 
COMMON/ PAR AMT / G, G2, GK1,GK2 


SLNTRG*SQRT (T  ACRRX( 1 ,N)**2+TACRRY  <  1,N )**2+ 
♦TACRRZ(1,N)**2) 

AZMITH»TRUHCG<TACRRXU,N),TACRRY(  l.N) ) 
GNDRNG«SQRT ITACRRX4 1 »N)**2+T  ACRRYI 1,N )**2) 
ELVATN»ATAN(TACRRZ( 1  »N) /GNDRNGi 

RN§PN§£lLNl^8iR*RSI GMA 
A-GGNCF(OSEEO) 

AZIPNS*AZM ITH+A*ASIGMA 
E*GGNQF( DSEED) 

EL VPN S»ELVATN+E*E SIGMA 

TACRMX(Nl»RNGPN§*CaS<gLVPNS)*SIN( AZIPNS) 


tMX( N l«RNGPNS*COS (  ELVPNS)*!  _  ... 

TACRMYl N)»RNGPNS*COS(cLVPNS)*COS< AZlPNS) 


END 


TACRMZ(N) 
RETURN 


«RNGPNS*SIN( EL VPNS) 


SU BROUT I NE  At  *=BTA  <  CT ,  HDGERR  ,  TTG  ) 

C0MMCN/TMRRST/N,TACRRX(3,1000),TACRRY(3,1000), 
♦  TACRRZC  3,1000) 

COMMCN/NOISE/TACRMXUOOO)  »TACRMY(  1000 )  ,TACRMZ< 
COMMON/ FI LTER/TACREX43, 1000 ) ,TACREY(3, 1000), 


10004 


♦TACREZ(3,1000) 

COMMON/ PR0CES/X2S IGM* Y2S IGM, Z2SIGM,EWVRRX* EWVRRY , 
♦  GX(IOOO)  ,GY<  1000)  ,GZ(1000)  , RA0RES(  1000)  ,XRES(  1000)  , 
♦YRESt 1000), ZR ESI  1000), Mll( 100 ) ,W22< 1 000) , W33 1 1000) , 
♦P( 1000,9) ,PINITL 


DT2=0T*0T 


I»=  (N  .NS.  1)  GC  TO  10 


INITIALIZE  FILTER. 
PPXKK=TACRMX(N) 
FOYKK*TACRMY(N) 
FPZKK=TACRPZ( N) 

FV  XKK*TACRRX( 2  ?  N ) +E  W VRRX 

FV  YK  K =TACRRY  (2, N)+ EWVRRY 

RVZKK=TACRRZ(2,N) 

F°XKK1*FPXKK+0T*FVXKK 

FOYKK1=FPYKK+OT*FVYKK 

FPZKK1=FPZKK*0T*FVZKK 

STRGER«0.0 

STRGRT*0.0 

MVPFLG*0 

MVRCNT*0 .0 


82 


iMiiiaaMai 


nnoo  ooo  oooon  non  onnon 


> 


60  TO  999 
CONTINUE 


8 


C 


40 

50 

C 

C 


C 

r 


SELECT  PARAMETERS  FOR  FILTERING  BASED  ON 
MANEUVERING  AND  NOISE  CRITERIA. 


HIGH  OR  LOW  NOISE  ENVIRONMENT? 


RUFRNG* SQRT(TACRMX(  N  )**2«-TACRMY  C  N)**2+TACRMZ  (  N  )**2 ) 
IF  (RUFRNG  .GT.  1500. J  60  TO  20 
N0SFLG»0 
GO  TO  30 
CONTINUE 
NOSFLG-1 
CONTINUE 


MANEUVERING? 

IF  TTG  .LT.  30  SEC  ASSUME  NO  MANEUVERING  AND  GO  ON. 


IF  (TTG  .LT.  3C.I  GO  TO  40 

CALL  STRG( CT.NOSFLG, FDGERR » STRGER.STRGRT) 
CALL  TMNVR( SfRGRT.MVRCNT ) 

|F  ( ( 0T*FLuAT ( MVRtNT )  )  .GT.  5.01  MVRFLG»0 


( ( CT*FLOAT (MVRCNT ) )  .LT.  -5.0)  MVRFLG»0 
IF  (ABS(HDGERR)  .GT.  .052359878)  MVRFL&-1 
60  TO  50 
CONTINUE 
MVRFLG*0 
CONTINUE 


FIND  ALFA/BETA  PARAMETERS  . 

CALL  CRSTRK(NOSFLG»MVRFLG*TTGtCALFA » CBETA ) 
GX (N) *CALFA 


GY (N ) *CALFA 
GZ(N)=CALFA 

FILTER  X-COORC.  DATA. 
CURRENT  STATES. 


XRES0U«TACRMX(N)-FPXKK1 
FPXKK*FPXKK1+CALFA*XRESDU 
FVXKK=F VXKK+CBETA*XRESDU/DT 

PREDICTEC  STATES. 

FPXKKl»FPXKK+FVXKK*DT 

FILTER  Y-COORO.  DATA. 
CURRENT  STATES. 


YR5S0U«TACRMY (N)-FPYKKl 
F°  YKK=FPYKK1+CALFA*YRESDU 
FVYKK*F VYKK+C  8ET  A* YRESDU/DT 

PREDICTED  STATES. 

FPYKK1»FPYKK+FVYKK*0T 

FILTER  Z-COORC.  DATA. 
CURRENT  STATES. 


nono  nnn  nnnn  o  non 


\  *.  v.  vv-v 


K-'- 

l> 


ZRESDU»TACRMZ (NI-FPZKK1 
FPZKK*FPZKK1*CALFA*ZRES0U 
FVZKK*FVZKK+CBET  A*ZRESOU/DT 

PREDICTED  STATES. 

FP  ZKK1*FPZKK*FVZKK*DT 

XRES<  Nl *XRESDU 
YRES<N)*YRESDU 
ZRESf  N)>ZRES0U 

RADRE$<N>*SQRT<XRES0U**2+YRESDU**2*ZRES0U**2i 
999  CONTINUE 

TACREXi 1 »N) *FPXKK 
TACREYC I»N )*FPYKK 

tacr£z< i,n)»fpzkk 

TACREX( 2»N )*F VXKK 
TACR EY ( 2»N)*FVYKK 
TACREZC  2  »N)*F VZKK 
RETURN 
END 


SUBROUTINE  STRG< OT , NOS FLG»HDGERR .STRGERt STRGRT ) 
IF  <  NOSFLG  .EQ.  1)  GO  TO  15 
ALFA-.0625 
BeTA* .0078125 
GO  TO  20 
CONTINUE 

ALFA*. 02125 
BETA*  .OC 19 53 12 5 
CONTINUE 

STRGER*STRGER*ALFA* (HDGERR-STRGER) 

STRGRT*STR  GRT+BETA* ( HDGERR-STRGER ) 

STRGER*STRGER+STRGRT*DT 

RETURN 

END 


SUBROUTINE  TMNVR(STRGRT »MVRCNT ) 

Ic  ( A0S( STRGRT )  .GT.  .00872665)  GG  TO  10 
IF  <MVRCNT.LT.  0)  MVRCNT*0 
MVRCNT-MVRCNT+1 
GO  TC  30 

IF  ( A0S  <  STRGRT )  .LT.  .0174533)  GO  TO  20 
IF  ( MVRCNT  .GT.  0)  MVRCNT-C 
.MVPCNT*  ^VRCNT-l 
GO  TO  30 
MNRCNT-0 
CONTINUE 
RETURN 

END 


SUBROUTINE  CRSTRKiNOSFLGtMVRFLGt  TTG»  CALFA  »C8ETA) 
IF  ( NCSCLG  .EQ.  0)  GO  TO  50 
IF  (MVRFLG  .EQ.  0)  GO  TO  10 
LOW  PRECISION  (MANEUVERING) 

CALF A*. 1875 
C8ETA*. 0078125 
GO  TC  30 


84 


non  r>  nno 


C 

C 


10 


20 


30 

50 


60 


C 

C 


70 


80 

99 


C 

C 


8 


END 


HIGH  PRECISION  4N0N-MANUEVERING) 

IF  CTTG  .LT.  30« ) GO  TO  20 

MANUEVERING  STOPPED  WITH  .GT.  30  SEC  TO  GO. 
CALFA*. 0390625 
CBETA*. 0002441406 
GO  TO  30 

CCNmInUEVERING  STOPPED  WITH  .LT.  30.  SEC  T3  GO. 
CALFA*. 078125 
CBETA*. 0009 765625 
GO  TO  99 
LOW  NOISE. 

C0NT^NYmVR FLG  .EQ.  0)  GO  TO  60 

£j]WpPRECISION  4 MANEUVERING) 

C8ET  A*Io312 5 
GO  TO  80 

C0Nh1ghEPRECISI0N  (NON-MANEUVERING) 

IF  ( TTG  .LT.  30.)  GO  TO  70 

MNVRNG  STOPPED  WITH  30  OR  MORE  SEC  TTG. 
CALFA*. 078125 
CBETA*. 0009765625 
GO  TO  80 

C0NMNVliNG  STOPPED  WITH  LESS  THAN  30  SEC  TTG. 
CALFA-. 15625 
CBETA*. 0039C625 
CONTINUE 
CONflNUE 
RETURN 


4  KADtOT »RSIGMA»ASIGMA*E SIGMA* RANGE* 


DOUBLE  PRECISION  CSEED 
COMMON/ DOUBLE /OS EEC 

COMMON /TMRR ST /N,TACRRX 43,1000),  TACRRY43 , 1000 ) , 
♦TACRRZ ( 3 ,1 QQO) 

C0MMON/N0IS£/TACRMX( 1000) , TACRMY (1000) , TACRMZ ( 1000 ) 
COMMON/ FILTER /TACR EX (3, 1000), TACREY( 3, 1000), 

♦  1CREZ4  3,1000 ) 

COMMON/ PR0CES/X2S I GM  *Y2SIGM ,Z2SI GM, E WVRRX ,5WVRRY , 
♦GX(IOOO) ,GY(1 COO), GZ( 1000) ,RAORES( 1000) ,XR£S( 1000) , 
♦YP=S(  10C0) ,ZRES( 1000) ,W11(1000) ,W22( 1000) 

♦ » W  33  (  100C) , °(  1000,9) ,PINITL 

DIMENSION  Q ( 6 , 6) , PKK ( 6 ,6) , R (6, 6)  ,W( 6 ,6 ) , G46 ,6 ) , 

♦  PH  I (6, 6) , DEL <6, 6) .01(6,6), NULL (6 ,6) ,H(6,6) ,XKK(6) , 

+  XKK1 (6) , CK 1(6), EXTRA  1(6, 6)  , EXTRA2< 6 , 6 ) , PKK I ( 6 , 6 > , 
♦WKAREA4 18) ,EXTRA3(3,3) , cXTRA4( 3 , 3) , Z (6) 


IF  (N  .NE.  1  )  GO  TO  25 
CT2*of*OT/2.0 


00 1?iU*:6o 


85 


I 


c 

c 


or>  10  j*i»  6 
OdtJi-o.o 
PKK4I ,J)-0.0 

IF  (I  .  EQ.  J)  PKM I,J)*PINITL 
R(ItJ|*0.0 
W(I,J)»0.0 
G(I,J)-0.0 
PHIU*J)*0.0 
DEL( I » J 1*0*0 

0IIWJli°:8a.  J)  dki , j)«i.o 

NULL ( I » J )»0.0 

io  cont}n&!**0’0 

20  CONTINUE 

ai-?iii8ss?iii8s 

W3*Z2SIGM*Z2S  IGM 

WX«0.0 

WY*WX 

VZ*WY 

IXFLG-0 

IYFLG-0 

DcL^ld  »0T2 
OFL( 2  t 1  *0T 
OE  L ( 3  *  2  -0T2 

8Ifc{t:it-oT2 

D5L(6*3 )*DT 
H( 1*1)-1.0 
H(2,3)*1.0 
H( 3*51*1.0 

pHia,  li-i.o 

PHI(1,2  *0T 

PHI(2*2I«1.Q 

PHI(3*3>*1.0 

PHI ( 3  *4 ) *0T 

PHI(4.4)  *1.0 

PHI  (  5  » 5 )*1 .0 

PH  I  (  5  *6  )  *0T 

PH  1(6*6) *1.0 

PVAR*RSIGMA*RSIGMA 

AVAR* AS I GMA*ASIGMA 

EVAR*ESIGMA*ESIGMA 

TACREX4  1* 1  )*?ACRMX( 1 ) 

XKK(  1 )  *T  ACREX ( 1  *  1 ) 
TACRSY(1,1)*TACRMY(1) 

XKK( 3 )*t  ACREY (1*1) 

TACREZ(1»1)*T  ACRMZ( 1 ) 

KcRFxlL^-f  i&£x<2  ? N ) +EWVRRX 
XKK(?)*TACREX(2,1) 
TACREY(2»1)*TACRRY(2»N) +EWVRRY 
XKK(  4)  «T  ACREY  (2*1) 
TACRSZ(2»1)*TACRRZ(2,N) 
XKK(6)«TACREZ(2, 1) 

IF  (N  .50.  1)  GO  TO  999 


25  CONTINUE 


Z( 1 ) *TACRMX( N) 
Z( 2) *TACRMY (N ) 
Z(  3)*TACRMZ(N) 
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non  on  no  ooo  no  nn 


R2«RANGE*RANGE 
CA«C0STA2MITH) 
$A*$IN( AZMITH ) 
CE»COS ( ELEVTN ) 
$E*SIN( IsLEVTN) 
CA2*C A*CA 
$A2»$A*SA 
CE2»CE*CE 
SE2«SE*SE 


P(lf  1)« 
R(2»2)> 
R(  3f  3)a 
R(  1,21* 

«!:«: 
R(  3,  l  )s 
R(2,3h 
R(  3, 2  )> 


R2*<£VAR*SE2*SA24AVAR*CE2*CA2)*RVAR*CE2*$A2 

R2*(EVAR*SE2*CA2+AVAR*CE2*$A2M‘RVAR*CE2*CA2 

R2*EVAR*CE2*RVAR*SE2 

R2*EVAR*$E2*SA*CA+( RVAR-R2*A VAR j*(CE2*$A*CAl 


jjV  AR*SE2*  SA2*AVAR*CE. 
:VAR*SE2*CA2+AVAR*CE; 


ffltaii  R2*EVAR1*SE*CE*SA 
R ( 1 »3) 

( RVAR— R2*EVAR 1*SE*CE*CA 
R(2»3) 


R2*EVAR1*SE*CE*SA 


FK1(1)»0.0 
FK  1(  2  )  *0.0 
FKK3I-0.0 
FK1(4)»0.0 
FK1(5)>0.0 
FK1(6J«0.0 

IF  (KAO  .NE.  0)  GG  TO  29 
W(1,U*W1 
W<2,2)*W2 
W(3,3  *W3 
GO  TC  34 
!9  CONTINUE 

IF  (KAO  .NE.  I)  GO  TO  31  ,  ^  „ 

CALL  AOPTV 1 (N,6 , 120 , Wl, W2 » W3 , TACREX,TACREY , TACREZ, 

+  WI 

GO  TC  34 
51  CONTI NUE 

1*=  (fTG  .LT.  20.1  GO  TO  33 
XRS*XRES(N-1) 

YRS*YRES(N-1) 

ZRS»ZRES(N-U 

CALL  A0PTV2(6tXRS, IXFLG,YRS» I YFLG, ZRS , I ZFLG, PKK1 
53  CONTINUE 

W(l, l)»Wl 
W( 2, 2 ) *W2 
W< 3, 3)*W3 
14  CONTINUE 

Hll(N)»W(lf 1) 

W22(  N  )»W  ( 2 , 2 ) 

W33(N)*W(3,3) 

GENERATE  Q-MATRIX. 

CALL  OUADRS ( DEL*  M , NULL ,6 ,6, Q1 


X(K/K-1)*PHI(K,  K- 1 1*X  (  K—  1/  K—  1)+F(K-1J 
CO  210  r*l,6 

XKK1 ( 1 1 »FK1 ( 1 1 
00  200  J*1 * 6 

XKKKI  )  *  XKK1 (  I  l  +  PHK  I  , J 1 *XKK( J  ) 


37 


non 


ONTI 
CONTINUE 


PC  K/K-l )«PH I (K/K-l )*P( K-1/K-1)*TRANSP0SE  C  PHI )  *Q(  K-l I 
CALL  QUADPS (PHI*  PKK ,Q*  6«6*PKK1 ) 


GCK) 


l-PCK/K-l |*H(K )*INV( HCK )*P(K/K-i )*TRAN$POSE(HI *R(KI } 
CALL  QUACPS»H*PKK1*R»6*6*EXtRA1 ) 

DO  212  I  -1*3 
00  211  J-1.3 

EXTRA4(I,J) -EXTRA1CI ,  J) 

CONTINUE 
CONTINUE 
DO  230  1-1*6 
00  225  J-l » 6 

EXTRA2C I »J )-0«0 
DC  220  K-l, 6 

EXTR A2C 1  *  J) -EXTRA2C I*  J I +PKK1C I,KI*H(J,KI 
CCNTINUE 
CONTINUE 
CCNTINUE 


INVERT  SXTRA1  ANO  MULTIPLY  BY  EXTRA2. 

CALL  LINV2F(EXTRA4*3 ,3  * EXTR A3, 1 DGT, WKAREA, IER ) 
CALL  PRCDCTC EX TRA2, EXTRA 3, 3,3,6 , G) 
GX(N)«G<1*1J* 

GY(N)*G(3,2> 

X(  K/Kfix!K?K-I?+G(K)*(Z(K)-H(KI*X( K/K-l ) ) 

DO  250  1-1,6 

EXTRA1CI,1)»2U1 
00  240  J  -1,6 

EXTRAK  I  *1 ) -EXTRA  1C  I » ll-HC  I , J )*XKK1( J ) 

it°0  C0NCT?S5  rE 

RADRESC  N  J-SQRT  (EXTRAIC  1, 1)**2+EXTRA1  C  2,  1J**2+ 
♦EXTR A1C 3,1)**2I 
XRESC  N  ) -EXTRAIC  1 , 1  i 
YRESC 


(N)-EXTRAl(2,l) 

(N)«EXTRA1C3,1) 


00  270  1-1,6 

XKKCI )«XKK1CI) 

DO  26C  J-l ,3 

XKKCII-XKKCn+GC  I ,  J  )*EXTRA  1C  J  ,  1) 
>60  CONTINUE 
>70  CCNTINUE 

TACRSX(1,N)»XKK( 1) 

TACREX(2,N)-XKK(2) 

TACREYC 1,N)»XKK( 3) 

TACRfYC2,N)»XKK(4) 

TACR^ZC l,N)»XKK( 5) 

TACREZ(2,N)»XKK(6) 

PC  K/KJ-PC K/K-11-G(K)*H(K)* PC K/K-l) 

CALL  PRC OCT (G,H,6,6,6, EXTRA1 ) 

CALL  PRCDCT <EXTRA1*PKK1 ,6, 6,6, EXTR A2 ) 
DO  290  1-1,6 
DO  280  J-l , 6 

>80 

>90  CONTINUE 
>99  CONTINUE 


P(  N*1 )-PKK(l* 1) 
P(N,2)»PKK(2,2) 
P(N,3I«PKK(3,3 
®(N,4)-PKK(4,4 
P( N*5 )  -PKK (5*5 
P( N*6  )*  PKK(6»  6 

RETURN 

ENO 


SUBROUTINE  KA L MN2< K AD , OT , RS IGM A , AS I GMA, E S IGMA , RANGE 
♦AZMITH* SLEVTN,TTG) 


OOUBLE  PRECISION  OSEED 
COMMON/OCUBLE /OSEEO 

COMMON/TMRRST /N»TACRRX< 3* 1000 ) * TACRRYC3 » 1000)  » 

+CoSmOn)n6i?§?IaCRMX<1000)»TACRMY(100Q),TACRMZ(1000) 
COMMON/ PIL TER /TACR EX(3, 1000) ,TACReY( 3,1000) , 

^COMMON/ pftoci^/X2SIGM  # Y2S IGM.Z2SI GM*  EWVRRX*EWVRRY* 
♦GX ( 1000 ) »GY ( 1 000  T.GZ (1000) , RAORES( 1000) . XRES ( 1000) t 

♦  YRES ( 1000) »ZP  ES(  1000) , Wll(lOOO) ,W22(1000) 

♦»W 33 (1000) *  P( 1000*9) tPINITL 

DIMENSION  Q( 9 *9) * PKK (9*9) » R(9»9) ,W(9,9) ,G(9,9 ) . 

+PH  I(<*,9)*DEL(9,9|»QI(9*9),  NULL  (9,9)  ,H(9»9)*XKK(9)  , 
)-XKKl(9)  tFKl(9),EXTRAl(9,9)  ,EXTRA2(9,9),PKK1(9,9) 

♦  ,WKAPEA( 75 ) , EXTRAS ( 3*3 ) »EXTRA4<  3*3) ,Z(9) 


IF  (N  .N5.  1  ) 
IDGT=2 

CT2=0T*0T/2.0 

0T3-DT2*CT/3.0 


)  GO  TO  25 


DO  20  1*1,9 
Z(I)«0.0 
DO  10  J*1 , 9 

PKK?f ! J?i8.0 
IF  (I  .EQ. 
R ( I, J l»0  .0 
W( I* J»*C.O 
G( !, JJ-0.0 

EKE!  1:113:8 
01  ‘f f 

NUtUI ,  JJ-0.0 
H(I, J)»0.0 
CONTINUE 
CONTINUE 

W1«X2SIGM*X2SIGM 

W2»Y2SIGM*Y2SIGM 

W3»Z2SIGM*Z2SIGM 

WX-0.0 

WY-WX 

WZ-WY 

IXFLG-O 

IYFLG-0 


PKK(  I 


'  PI NI TL 


Cl  ( I*J)-1.0 


non 


ooo  oo  no  non  no 


29 


31 


33 


34 


SE«SIN(ELEVTN) 
CA2»CA*CA 
SA2*SA*$A 
£E2«CE*CE 


SE2*SE*SE 


RCitli 

Si!: i I 

R(  1, 2)> 

Si!:!l 

R( 3,1 ) 


■R2*<EVAR*SE2*$A2+AVAR*CE2*CA2)+RVAR*CE2*SA2 
•R2*( EVAR*SE2*CA2*AVAR*CE2*SA2) ♦RVAR*CE2*CA2 


«R2*§VAR*CE2*RVAR*$E2 
R2*EVAR*SE2*SA*CA*< RVAR- 


R2*AVARJ*(CE2*SA*CA) 


R(  3,2 


(RVAR-R2*EVAR)*SE*CE*SA 

«(^vAr-P2*EVAR )*SE*CE*CA 
>R(2,3) 


FK1<1 

FK1(2 


FK 

FK 

FK 

FK 

FK 

FK 

FK 


(3 

(4 

(5 

(6 

(7 

(8 

(9 


•0.0 

■0.0 

“8:8 

■0.0 

>0.0 

■0.0 

>0.0 

■0.0 


IF 


0)  GO  TO  29 


(KAO  .NE. 

W(1,1)*W1 
W<2,2I*W2 
W<3,3)*W3 
GO  TC  34 
CONTINUE 

IP  (KAD  .NE.  1)  GO  TO  31 

CALL  ADPTV 1(N»9,120»W1,W2,W3, TACREX, TACREY,  TAC  REZ » 

+  H) 

GO  TC  34 
CONTINUE 

IF  ( TTG  .LT.  20.) 

XRS*XRES<N-1) 

YRS-YRES(N-l) 

ZRS*ZRES(N-1) 

CALL  A0PTV2(9,XRS.IXFLG»YRS» I YFLG, ZRS, I ZFLG, PKK) 
CONTINUE 
W< l » i ) *R1 
W( 2t  2 )*W2 
H(3, 3  )*W3 
CONTINUE 
W11(N)*W<1,1) 

W22( N) *W (2 ,2) 

W33<N)*W<  3,3) 


GO  TO  33 


GENERATE  Q-MATRIX. 

CALL  QU A0PS<0EL,W,NULL,9,9,Q) 


X(K/K-1)«PHI(K,K-1)*X(K-1/K-1)+F(K-1) 

DO  210  1*1,9 

XKK1 ( I ) *FK1( I ) 

DO  200  J*1  ♦  9 

XKK1 ( I ) *XKK1 ( I )+PHl( I, J)*XKK( J  ) 
CONTINUE 
CONTINUE 


200 

210 


P<K/K-1I-PHIIK/K-1)*PIK-1/K-1I*TRANSP0SE(PHI  l*Q< K-ii 
CALL  QUAOPSI PHI ,PKK, Q,9«9, PKK1 ) 


GI K )«P (K/K— 1 )*HI  K  J  *1 NV (H(Ki*P<K/K-l )*TRANSPOSE(H ) +R(K )  J 
CALL  QUAOPS (H»PKK1 *R *9,  9 1 EXTRA I) 

DO  212  I  *1,3 
00  211  J-l *3 

EXTRA4< I,J)«EXTRA1(I,JJ 

211  CONTINUE 

212  CONTINUE 

00  230  1*1,9 
00  225  J-l ,9 

EXTRA2C I, JI-0.0 
00  220  K*l,9 

EXTRA2(f  , JJ-EXTRA2CI, J ) ♦PKK1 ( I , KJ*HI J,K) 
220  CONTINUE 

225  CONTINUE 

230  CONTINUE 


C  INVERT  EXTRA1  AND  MULTIPLY  BY  EXTRA2. 

CALL  LI NV2F (EXTRA4»3*3 » EXTRA3, I DGT*  WKAREA* IER I 
CALL  PROOCT ( EXTRA2 , EXTRA3, 3 ,3,9, G) 

GX (N)*G ( 1, 1 ) 

GY  (N) «G ( 4,2 ) 

GZ(N)-G(7.3» 

C  X( K/K) -XI K/K-l ) +G (K)*( ZC  K)— F(K)*X( K/K-1J ) 

DO  250  1*1,9 

EXTRAlf  I,l)*ZUI 
DO  240  J  -1,9 

EXTRAK  I  ,1  J*EXTRA1(I  »1J-H(I,J|  *XKK1  ( J  ) 
240  CONTINUE 

250  RAOR^sVn  |*$QRT(EXTRAK  1 » 1J  442+EXTRA1  (2,1  >++2+ 
♦EXTRA1 ( 3 ,11**2 ) 

XRES ( N) -EXTRA HI  ,  1 ) 

‘ ]5(N ) -EXTRA  1(2,1) 

:S<Ni-EXTRAl(3,l) 

270  1-1,9 
XKKI  I  l-XKKKI  ) 

DO  260  J-1,3 

XKKC I)- XKKI I J+G( I , JJ*EXTRA1( J , 1 ) 

260  CONTINUE 

270  CONTINUE 

TACREX( 1»N)»XKK( 1) 

,,N  -XKKI2 
l,N)-XKK(  3  J 

1 , N ) -XKKC 4) 

2, N)-XKKC5J 

3, NI-XKK<  6/ 

1 ,N)«XKK( 7) 

2  »N ) -XK  K< 8) 


SI 

DO 


TACREXj 


TACR6X 

TACREYI 

TACREYI 

TACREYI 

TACR5Z I 

TACREZI 


TACREZI  3,N  J-XKKI  9J 


v# 

8 


P(K/K)-P(K/K-n-G(K)*H(KJ*P(K/K-l) 

CALL  PRCCCTIG,H»9,9, 9, EXTRA1 i 
CALL  PR0CCT(EXTRA1,PKK1,9,9,9,EXTRA2I 
DO  290  1-1,9 
DO  280  J-l  ,9 

PKKI I, Jl-PKKlt I, JJ-EXTRA2I I»J) 
280  CONTINUE 
290  CONTI NU  5 
999  CONTINUE 
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1  N,1 

(»PKKI 

[1,1) 

1  N»2 

■  PKKI 

[I:U 

i  N*  3 

>*PKK 

<N*4 

«PKK< 

4,4) 

C  N»5 

■  PKKI 

5,5) 

<N,6 

■  PKKI 

6,6) 

IN, 7 

■  PKKI 

7,7) 

IN,  8 

■  PKK 

18,8) 

IN, 9 

■  PKKI 

9,9) 

FORMAT I •  ,,*GI*»I1»*»,»I1»' )*• »  F20.10) 

RETURN 

END 


SUBROUTINE  QUAOPS 1 X,H, R,M,N,C) 

DIMENSION  XIM,N),H(N,N),R{M,M),CIM,MJ 
DO  400  I  ■  1,M 
00  300  J*  1»  H 

00  100  L*1»N 

M  Cljt Jl-CII*JI+XU*M*H<K»lJ*XCJfU 
CONTINUE 
CONTINUE 
CONTINUE 
CONTINUE 
RETURN 

ENO 


SUBROUTINE  A0PTV1 IN, 13 , JSTART, Ml ,W2, H3.TACREX, TACREY , 
♦TACREZ* H) 

♦well  if  IH8WSHS}  lit0001  ’ TACRE¥<3'10001’ 

ISTATE-I3/3 

IF  (N  .LT.  JSTART )  GO  TO  27 

WX»8 .0*ITACREX I I  STATE »N-1 ) -TACREXC I  STATE ,N-2) J 
W  Y=8«  C* I TACREY 1 1  STATE  »N— 1 ) — TACREY! I  STATE »N-2) I 
W Z-8 . 0* I TAC R EZ II  STATE , N-l ) -TAC R EZ 1 1 STA T E , N-2 ) ) 
GO  TO  35 
CONTINUE 
WX-0.0 
WY»Q  *0 
WZ*0.0 
CONTINUE 

IF  (  ISTATE  .EO.  3)  GC  TO  37 
Wtl,l)«Wl+l  WX*NX)/3. 

Wl2,2)*W2+lWY*WY)/3. 

W<3,3)-W3+IWZ*WZ)/3. 

GO  TC  41 
CONTINUE 

W( U l)*fcH-IWX*WX)/81. 

W  ( 2,  2  |*M2'M  WYJWY ) / 81 • 

A  l3'3)*ta3+(WZ*WZi/81. 

CONTINUE 

RETURN 

ENO 


SUBROUTINE  ADPTV2 (NORDERtXRESt I XFLG, YRES t IYFLG,ZRES, 

ON  PKK  CNCROER  tNOROER J 
RESET-1000. 
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CALL  81  AS ( X RES * IXFLG ) 
IF  (IABSUXFLG)  .LT.  i 
PKK(1,1)*RESET 


PKK<  2,2 )»RESET 
IF  (NORCER  .SQ.  91  PKK(3,3)»R 
CONTINUE  „  _ 

CALL  BIAS! VRES * IYFLGJ  ,  „  M 
IF  ( IABS(I YFLG)  .LT.  8)  GO  TO  14 
IF  (NORCER  .EQ.  9)  GO  TO  12 
PKK(3,3)*R£SET 
PKK(4*4)*RcsiT 
GO  TO  14 
CONTINUE 
PKK(4,4T«RE$ET 
PKK(5,5)»RESET 
PKK(6,6)»RESET 
CONTINUE 

wiEve*,,  go  to  2o 

IF  (NOROER  .EO.  9)  GO  TO  16 
PKK(5,5)»REigT 
PKK(6,6)«RESET 
GO  TO  20 
CONTINUE 
PKK<7,7  i*P.ESET 
PKK(8,  8J=RESET 
PKK(9,9)=RESET 
CONTINUE 
RETURN 


'4os? 


TO  14 
0  12 


i&iifT 

•RESET 


SUBROUTINE  BI AS( RE SI  DU » I  FLAG ) 

IF  ( RESIOU )  10,20*30 

IF  ( I  FLAG  .LE.  0)  GO  TO  12 
IFLAG-0 
GO  TO  40 
CONTINUE 

I FLAG-I FLAG-1 
GO  TO  40 
IFLAG*0 
GO  TO  40 

IF  (IFLAG  .GE.  Oi  GO  TO  32 
IFLAG*0 
GO  TC  40 
CONTINUE 
IFLAG*I FLAG+1 
CONT INUE 
RETURN 


RESET 


‘I  FLAG+1 
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